diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-14 10:38:34 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-14 10:38:34 +0200 |
commit | 51a4ef5de1bc542ac4f7072d95250cd62ea73ed6 (patch) | |
tree | b71db4faea6a0ac39e4fa28481421a2acc13a896 /src/modules/position_estimator_inav/inertial_filter.c | |
parent | 5e0911046173e01a6c66b91d3e38212e093159d0 (diff) | |
parent | ddc8f1fa5f5b88549af5e4f5f46c751a5f3af3ce (diff) | |
download | px4-firmware-51a4ef5de1bc542ac4f7072d95250cd62ea73ed6.tar.gz px4-firmware-51a4ef5de1bc542ac4f7072d95250cd62ea73ed6.tar.bz2 px4-firmware-51a4ef5de1bc542ac4f7072d95250cd62ea73ed6.zip |
merged upstream/master into sbus2_sensorssbus2_sensors
Diffstat (limited to 'src/modules/position_estimator_inav/inertial_filter.c')
-rw-r--r-- | src/modules/position_estimator_inav/inertial_filter.c | 32 |
1 files changed, 17 insertions, 15 deletions
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 13328edb4..a36a4688d 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -5,27 +5,29 @@ * Author: Anton Babushkin <rk3dov@gmail.com> */ +#include <math.h> + #include "inertial_filter.h" -void inertial_filter_predict(float dt, float x[3]) +void inertial_filter_predict(float dt, float x[2], float acc) { - x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; - x[1] += x[2] * dt; + if (isfinite(dt)) { + if (!isfinite(acc)) { + acc = 0.0f; + } + x[0] += x[1] * dt + acc * dt * dt / 2.0f; + x[1] += acc * dt; + } } -void inertial_filter_correct(float e, float dt, float x[3], int i, float w) +void inertial_filter_correct(float e, float dt, float x[2], int i, float w) { - float ewdt = w * dt; - if (ewdt > 1.0f) - ewdt = 1.0f; // prevent over-correcting - ewdt *= e; - x[i] += ewdt; - - if (i == 0) { - x[1] += w * ewdt; - x[2] += w * w * ewdt / 3.0; + if (isfinite(e) && isfinite(w) && isfinite(dt)) { + float ewdt = e * w * dt; + x[i] += ewdt; - } else if (i == 1) { - x[2] += w * ewdt; + if (i == 0) { + x[1] += w * ewdt; + } } } |