diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-11 12:51:34 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-11 12:51:34 +0100 |
| commit | 4d8524f508fe9e74606fe80f063801a1c47baa14 (patch) | |
| tree | 7092a00025bb0f229936a44999e0c10255a60219 /src/modules/position_estimator_inav | |
| parent | 16dd054fa6bfc305d3a66f17d295ceaf1f554c12 (diff) | |
| parent | b6a087e92183ef9cca513357919b96ea2812d298 (diff) | |
| download | px4-firmware-4d8524f508fe9e74606fe80f063801a1c47baa14.tar.gz px4-firmware-4d8524f508fe9e74606fe80f063801a1c47baa14.tar.bz2 px4-firmware-4d8524f508fe9e74606fe80f063801a1c47baa14.zip | |
Merge branch 'master' of github.com:PX4/Firmware into paul_estimator
Diffstat (limited to 'src/modules/position_estimator_inav')
| -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 37f507086..368fa6ee2 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 */ |