fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 06:43:33 +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/baro.cpp
+5 -7
View File
@@ -85,13 +85,11 @@ void BlockLocalPositionEstimator::baroCorrect()
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK");
}
// kalman filter correction if no fault
if (!(_sensorFault & SENSOR_BARO)) {
Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
// kalman filter correction always
Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
void BlockLocalPositionEstimator::baroCheckTimeout()