dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fix for lpe accel bias saturation. (#5466) 

* 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
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);
}
}