diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-09 22:08:28 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-09 22:08:28 +0400 |
| commit | 647142764fa3ffd5d99f4d43950975cc6a19bded (patch) | |
| tree | 7463bbbd2800e4b1c2da581451f3708724b108f0 /src | |
| parent | 501dc0cfa7259a1916522e5b70a5fd31cb7d20e1 (diff) | |
| download | px4-firmware-647142764fa3ffd5d99f4d43950975cc6a19bded.tar.gz px4-firmware-647142764fa3ffd5d99f4d43950975cc6a19bded.tar.bz2 px4-firmware-647142764fa3ffd5d99f4d43950975cc6a19bded.zip | |
position_estimator_inav: hotfix, change lower dt limit from 5 ms to 2 ms
Diffstat (limited to 'src')
| -rw-r--r-- | src/modules/position_estimator_inav/inertial_filter.c | 5 | ||||
| -rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 |
2 files changed, 2 insertions, 5 deletions
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 13328edb4..7cd076948 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -15,10 +15,7 @@ void inertial_filter_predict(float dt, float x[3]) void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = w * dt; - if (ewdt > 1.0f) - ewdt = 1.0f; // prevent over-correcting - ewdt *= e; + float ewdt = e * w * dt; x[i] += ewdt; if (i == 0) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d6d03367b..a14354138 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -623,7 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; - dt = fmaxf(fminf(0.02, dt), 0.005); + dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms t_prev = t; /* use GPS if it's valid and reference position initialized */ |