diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-01 22:43:56 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-01 22:43:56 +0400 |
commit | e06263f76f9e38ea9624f1f1ab21af8731a3cd7f (patch) | |
tree | 398489082bcebc98cbe7b3c3bb11dbcf86091e34 | |
parent | 2c337d9b456422b908b9be647de0fd4f6a920e21 (diff) | |
download | px4-firmware-e06263f76f9e38ea9624f1f1ab21af8731a3cd7f.tar.gz px4-firmware-e06263f76f9e38ea9624f1f1ab21af8731a3cd7f.tar.bz2 px4-firmware-e06263f76f9e38ea9624f1f1ab21af8731a3cd7f.zip |
position_estimator_inav: failsafe against NaN estimate
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 14 |
1 files changed, 12 insertions, 2 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 ad363efe0..fb68dd31c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -708,6 +708,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); + float x_est_prev[3], y_est_prev[3]; + + memcpy(x_est_prev, x_est, sizeof(x_est)); + memcpy(y_est_prev, y_est, sizeof(y_est)); + if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); @@ -715,7 +720,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); - thread_should_exit = true; + memcpy(x_est, x_est_prev, sizeof(x_est)); + memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ @@ -739,7 +745,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); - thread_should_exit = true; + memcpy(x_est, x_est_prev, sizeof(x_est)); + memcpy(y_est, y_est_prev, sizeof(y_est)); + memset(corr_acc, 0, sizeof(corr_acc)); + memset(corr_gps, 0, sizeof(corr_gps)); + memset(corr_flow, 0, sizeof(corr_flow)); } } |