fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 04:43:34 +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/baro.cpp
+3 -10
View File
@@ -76,8 +76,8 @@ void BlockLocalPositionEstimator::baroCorrect()
if (beta > BETA_TABLE[n_y_baro]) {
if (_baroFault < FAULT_MINOR) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
//double(r(0)), double(beta));
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
_baroFault = FAULT_MINOR;
}
@@ -90,14 +90,7 @@ void BlockLocalPositionEstimator::baroCorrect()
if (_baroFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
if (!_canEstimateXY) {
dx(X_x) = 0;
dx(X_y) = 0;
dx(X_vx) = 0;
dx(X_vy) = 0;
}
correctionLogic(dx);
_x += dx;
_P -= K * C * _P;
}