baro.cpp
2.57 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
#include "../BlockLocalPositionEstimator.hpp"
#include <systemlib/mavlink_log.h>
#include <matrix/math.hpp>
extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
static const uint32_t REQ_BARO_INIT_COUNT = 100;
static const uint32_t BARO_TIMEOUT = 100000; // 0.1 s
void BlockLocalPositionEstimator::baroInit()
{
// measure
Vector<float, n_y_baro> y;
if (baroMeasure(y) != OK) {
_baroStats.reset();
return;
}
// if finished
if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
_baroAltOrigin = _baroStats.getMean()(0);
mavlink_log_info(&mavlink_log_pub,
"[lpe] baro init %d m std %d cm",
(int)_baroStats.getMean()(0),
(int)(100 * _baroStats.getStdDev()(0)));
_sensorTimeout &= ~SENSOR_BARO;
_sensorFault &= ~SENSOR_BARO;
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOriginGlobal = false;
_altOrigin = _baroAltOrigin;
}
}
}
int BlockLocalPositionEstimator::baroMeasure(Vector<float, n_y_baro> &y)
{
//measure
y.setZero();
y(0) = _sub_airdata.get().baro_alt_meter;
_baroStats.update(y);
_time_last_baro = _sub_airdata.get().timestamp;
return OK;
}
void BlockLocalPositionEstimator::baroCorrect()
{
// measure
Vector<float, n_y_baro> y;
if (baroMeasure(y) != OK) { return; }
// subtract baro origin alt
y -= _baroAltOrigin;
// baro measurement matrix
Matrix<float, n_y_baro, n_x> C;
C.setZero();
C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir.
Matrix<float, n_y_baro, n_y_baro> R;
R.setZero();
R(0, 0) = _param_lpe_bar_z.get() * _param_lpe_bar_z.get();
// residual
Matrix<float, n_y_baro, n_y_baro> S_I =
inv<float, n_y_baro>((C * m_P * C.transpose()) + R);
Vector<float, n_y_baro> r = y - (C * _x);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_baro]) {
if (!(_sensorFault & SENSOR_BARO)) {
mavlink_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
_sensorFault |= SENSOR_BARO;
}
} else if (_sensorFault & SENSOR_BARO) {
_sensorFault &= ~SENSOR_BARO;
mavlink_log_info(&mavlink_log_pub, "[lpe] baro OK");
}
// kalman filter correction always
Matrix<float, n_x, n_y_baro> K = m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
m_P -= K * C * m_P;
}
void BlockLocalPositionEstimator::baroCheckTimeout()
{
if (_timeStamp - _time_last_baro > BARO_TIMEOUT) {
if (!(_sensorTimeout & SENSOR_BARO)) {
_sensorTimeout |= SENSOR_BARO;
_baroStats.reset();
mavlink_log_info(&mavlink_log_pub, "[lpe] baro timeout ");
}
}
}