diff options
| author | Antonio Sanniravong <antonio.sanniravong@polymtl.ca> | 2014-06-24 07:47:16 -0400 |
|---|---|---|
| committer | Antonio Sanniravong <antonio.sanniravong@polymtl.ca> | 2014-06-24 07:47:16 -0400 |
| commit | 179bace35aaa32a23ebd7a9cb99d54eae53690f5 (patch) | |
| tree | 0cedb2a4469a2a6099f30ce2811452b65830f1f6 /src/modules/position_estimator_inav/position_estimator_inav_main.c | |
| parent | 68442e31ac6970be91592282c9b70ebc76fa142d (diff) | |
| download | px4-firmware-179bace35aaa32a23ebd7a9cb99d54eae53690f5.tar.gz px4-firmware-179bace35aaa32a23ebd7a9cb99d54eae53690f5.tar.bz2 px4-firmware-179bace35aaa32a23ebd7a9cb99d54eae53690f5.zip | |
Resets 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 f908d7a3b..5f132453b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -916,6 +916,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_reset_v); + inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_reset_v); } /* detect land */ @@ -931,6 +935,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) { |