fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 23:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

LPE Variance Dependent Publication (#4914)

Browse Source 
* Use variance to control publishing for LPE.

* Don't stop publishing if we have gps/ baro.

* LPE tuning and cleanup.

* Added bias saturation to LPE.

* Added vector enabled low pass filter block.

* Added rk4 integration and pub lowpass to LPE.

* Fix std::abs issue on mac/ reset lowpass on state reset.

* Don't estimate gyro bias when rotating at high speed  att_est_q.

* Lowered low pass on position to 5 Hz for LPE.

* Streamline state space update for LPE.

* Added health flags to est2 log.

* Revert to old tuning, more conservative, less faults.

* Formatting.

* Fix for fault message on LPE.

* Added subscription throttling to LPE.

* Formatting.
This commit is contained in:
James Goppert
2016-07-01 11:43:09 -04:00
committed by GitHub
parent 2a395c3fec
commit 00dfc99e08
17 changed files with 417 additions and 247 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/gps.cpp
+24 -27
View File
@@ -8,11 +8,25 @@ extern orb_advert_t mavlink_log_pub;
// to initialize
static const uint32_t REQ_GPS_INIT_COUNT = 10;
static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s
static const float GPS_EPH_MIN = 2.0; // m, min allowed by gps self reporting
static const float GPS_EPV_MIN = 2.0; // m, min allowed by gps self reporting
void BlockLocalPositionEstimator::gpsInit()
{
// check for good gps signal
uint8_t nSat = _sub_gps.get().satellites_used;
float eph = _sub_gps.get().eph;
float epv = _sub_gps.get().epv;
uint8_t fix_type = _sub_gps.get().fix_type;
if (
nSat < 6 ||
eph > _gps_eph_max.get() ||
epv > _gps_epv_max.get() ||
fix_type < 3
) {
_gpsStats.reset();
return;
}
// measure
Vector<double, n_y_gps> y;
@@ -50,21 +64,6 @@ void BlockLocalPositionEstimator::gpsInit()
int BlockLocalPositionEstimator::gpsMeasure(Vector<double, n_y_gps> &y)
{
// check for good gps signal
uint8_t nSat = _sub_gps.get().satellites_used;
float eph = _sub_gps.get().eph;
float epv = _sub_gps.get().epv;
uint8_t fix_type = _sub_gps.get().fix_type;
if (
nSat < 6 ||
eph > _gps_eph_max.get() ||
epv > _gps_epv_max.get() ||
fix_type < 3
) {
return -1;
}
// gps measurement
y.setZero();
y(0) = _sub_gps.get().lat * 1e-7;
@@ -125,11 +124,11 @@ void BlockLocalPositionEstimator::gpsCorrect()
float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();
// if field is not below minimum, set it to the value provided
if (_sub_gps.get().eph > GPS_EPH_MIN) {
if (_sub_gps.get().eph > _gps_xy_stddev.get()) {
var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
}
if (_sub_gps.get().epv > GPS_EPV_MIN) {
if (_sub_gps.get().epv > _gps_z_stddev.get()) {
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
}
@@ -170,13 +169,9 @@ void BlockLocalPositionEstimator::gpsCorrect()
if (beta > BETA_TABLE[n_y_gps]) {
if (_gpsFault < FAULT_MINOR) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta));
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
//double(r(0)), double(r(1)), double(r(2)),
//double(r(3)), double(r(4)), double(r(5)));
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
//double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)),
//double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5)));
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
double(r(3)*r(3) / S_I(3, 3)), double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
_gpsFault = FAULT_MINOR;
}
@@ -188,7 +183,9 @@ void BlockLocalPositionEstimator::gpsCorrect()
// kalman filter correction if no hard fault
if (_gpsFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
_x += K * r;
Vector<float, n_x> dx = K * r;
correctionLogic(dx);
_x += dx;
_P -= K * C * _P;
}
}