From e06263f76f9e38ea9624f1f1ab21af8731a3cd7f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 22:43:56 +0400 Subject: [PATCH 1/2] position_estimator_inav: failsafe against NaN estimate --- .../position_estimator_inav_main.c | 14 ++++++++++++-- 1 file 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 ad363efe06..fb68dd31c0 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)); } } From d11235585cf1014c5391b63e394151c171ed286d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 22:56:02 +0400 Subject: [PATCH 2/2] position_estimator_inav: log writing on NaN estimate fixed --- .../position_estimator_inav_main.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 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 fb68dd31c0..d6d03367b6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -167,12 +167,13 @@ void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], FILE *f = fopen("/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); - snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); - fputs(f, s); - snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); - fputs(f, s); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); + fwrite(s, 1, n, f); + n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + fwrite(s, 1, n, f); free(s); } + fsync(fileno(f)); fclose(f); }