diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-01 13:45:50 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-01 13:45:50 +0200 |
commit | 8f957aeb5f8f7527d1ef2a5583f4de7870c60513 (patch) | |
tree | a338b1f20419d7fc295175542ca384046356a0d4 /src/modules/position_estimator_inav/position_estimator_inav_main.c | |
parent | 545e207d13541c1f36325fcfa0f847d5f78ff4e3 (diff) | |
parent | 7a45888e78b4af0c04014c20e579797b2040edf8 (diff) | |
download | px4-firmware-8f957aeb5f8f7527d1ef2a5583f4de7870c60513.tar.gz px4-firmware-8f957aeb5f8f7527d1ef2a5583f4de7870c60513.tar.bz2 px4-firmware-8f957aeb5f8f7527d1ef2a5583f4de7870c60513.zip |
Merge pull request #1074 from t0ni0/reset_xy_v
INAV: Reset XY velocities when we can't estimate them
Diffstat (limited to 'src/modules/position_estimator_inav/position_estimator_inav_main.c')
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 7 |
1 files changed, 7 insertions, 0 deletions
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 db1ce79ca..05eae047c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -915,6 +915,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(y_est_prev, y_est, sizeof(y_est)); } + } else { + /* gradually reset xy velocity estimates */ + inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); + inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } /* detect land */ @@ -930,6 +934,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed = false; landed_time = 0; } + /* reset xy velocity estimates when landed */ + x_est[1] = 0.0f; + y_est[1] = 0.0f; } else { if (alt_disp2 < land_disp2 && thrust < params.land_thr) { |