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

LPE fault relaxation and sitl fix (#6146)

Browse Source 
* Set LPE FUSE for standard iris sitl config.

* Relax fault detection handling.

* Always correct lidar.
This commit is contained in:
James Goppert
2016-12-23 15:08:37 -05:00
committed by GitHub
parent 661fda2b2a
commit c28cd76e5f
9 changed files with 37 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/gps.cpp
+9 -8
View File
@@ -190,7 +190,10 @@ void BlockLocalPositionEstimator::gpsCorrect()
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_gps]) {
// artifically increase beta threshhold to prevent fault during landing
float beta_thresh = 1e2f;
if (beta / BETA_TABLE[n_y_gps] > beta_thresh) {
if (!(_sensorFault & SENSOR_GPS)) {
mavlink_log_critical(&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)),
@@ -203,13 +206,11 @@ void BlockLocalPositionEstimator::gpsCorrect()
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
}
// kalman filter correction if no hard fault
if (!(_sensorFault & SENSOR_GPS)) {
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
// kalman filter correction always for GPS
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
void BlockLocalPositionEstimator::gpsCheckTimeout()