fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 06:37:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix for lpe accel bias saturation. (#5466)

Browse Source 
* Fix for lpe accel bias saturation.

* Formatting.
This commit is contained in:
James Goppert
2016-09-07 23:53:50 +00:00
committed by GitHub
parent 3163cf90c9
commit 76c2d92b7e
1 changed files with 12 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+12 -3
View File
@@ -607,11 +607,20 @@ void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
float by = dx(X_by) + _x(X_by);
float bz = dx(X_bz) + _x(X_bz);
if (std::abs(bx) > BIAS_MAX) { bx = BIAS_MAX * bx / std::abs(bx); }
if (std::abs(bx) > BIAS_MAX) {
bx = BIAS_MAX * bx / std::abs(bx);
dx(X_bx) = bx - _x(X_bx);
}
if (std::abs(by) > BIAS_MAX) { by = BIAS_MAX * by / std::abs(by); }
if (std::abs(by) > BIAS_MAX) {
by = BIAS_MAX * by / std::abs(by);
dx(X_by) = by - _x(X_by);
}
if (std::abs(bz) > BIAS_MAX) { bz = BIAS_MAX * bz / std::abs(bz); }
if (std::abs(bz) > BIAS_MAX) {
bz = BIAS_MAX * bz / std::abs(bz);
dx(X_bz) = bz - _x(X_bz);
}
}