diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-06-28 11:10:46 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-06-28 11:10:46 +0400 |
| commit | 1759f30e3aa4b2da25dcd0c836480f069d917a88 (patch) | |
| tree | 808b1525d6bf684b5e00d51c7b446fe0186ec864 /src/modules/position_estimator_inav/inertial_filter.c | |
| parent | 665a9b8865b394c1e4d15d74632551dfd216eab9 (diff) | |
| download | px4-firmware-1759f30e3aa4b2da25dcd0c836480f069d917a88.tar.gz px4-firmware-1759f30e3aa4b2da25dcd0c836480f069d917a88.tar.bz2 px4-firmware-1759f30e3aa4b2da25dcd0c836480f069d917a88.zip | |
Sonar added to position_estimator_inav
Diffstat (limited to 'src/modules/position_estimator_inav/inertial_filter.c')
| -rw-r--r-- | src/modules/position_estimator_inav/inertial_filter.c | 7 |
1 files changed, 5 insertions, 2 deletions
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index acaf693f1..13328edb4 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -13,9 +13,12 @@ void inertial_filter_predict(float dt, float x[3]) x[1] += x[2] * dt; } -void inertial_filter_correct(float edt, float x[3], int i, float w) +void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = w * edt; + float ewdt = w * dt; + if (ewdt > 1.0f) + ewdt = 1.0f; // prevent over-correcting + ewdt *= e; x[i] += ewdt; if (i == 0) { |