diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-21 18:49:39 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-21 18:49:39 +0400 |
| commit | b2f8fcb9d904901127b937551c149e904c8aa3a9 (patch) | |
| tree | eebfb8f6303e8bb95815b3adf927a7790361a980 /src/modules/position_estimator_inav | |
| parent | 49bdfa8cfb00ad671500b3d32c101e6cbf0fb03d (diff) | |
| download | px4-firmware-b2f8fcb9d904901127b937551c149e904c8aa3a9.tar.gz px4-firmware-b2f8fcb9d904901127b937551c149e904c8aa3a9.tar.bz2 px4-firmware-b2f8fcb9d904901127b937551c149e904c8aa3a9.zip | |
position_estimator_inav: added NaN checks
Diffstat (limited to 'src/modules/position_estimator_inav')
| -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; + } } } |