diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-03-21 17:28:40 +0100 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-03-21 17:28:40 +0100 |
| commit | 0cc7270b83f3e8224ee189f76ea820e1165ecef2 (patch) | |
| tree | b1b25a9a49d16f0c547888529f6ea0f1d7aa6354 | |
| parent | 057bcf3172f3c8d1bab561e7e4cad14977cd74d0 (diff) | |
| parent | b2f8fcb9d904901127b937551c149e904c8aa3a9 (diff) | |
| download | px4-firmware-0cc7270b83f3e8224ee189f76ea820e1165ecef2.tar.gz px4-firmware-0cc7270b83f3e8224ee189f76ea820e1165ecef2.tar.bz2 px4-firmware-0cc7270b83f3e8224ee189f76ea820e1165ecef2.zip | |
Merge pull request #764 from PX4/inav_nan_checks
position_estimator_inav: added NaN checks
| -rw-r--r-- | src/modules/position_estimator_inav/inertial_filter.c | 24 |
1 files changed, 15 insertions, 9 deletions
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 7cd076948..2f1b3c014 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -5,24 +5,30 @@ * Author: Anton Babushkin <rk3dov@gmail.com> */ +#include <math.h> + #include "inertial_filter.h" void inertial_filter_predict(float dt, float x[3]) { - x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; - x[1] += x[2] * dt; + if (isfinite(dt)) { + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; + } } void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = e * w * dt; - x[i] += ewdt; + if (isfinite(e) && isfinite(w) && isfinite(dt)) { + float ewdt = e * w * dt; + x[i] += ewdt; - if (i == 0) { - x[1] += w * ewdt; - x[2] += w * w * ewdt / 3.0; + if (i == 0) { + x[1] += w * ewdt; + x[2] += w * w * ewdt / 3.0; - } else if (i == 1) { - x[2] += w * ewdt; + } else if (i == 1) { + x[2] += w * ewdt; + } } } |