diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-01 22:56:02 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-01 22:56:02 +0400 |
commit | d11235585cf1014c5391b63e394151c171ed286d (patch) | |
tree | bc0985ec5b5ff43d9c7d1d4c244fefc46fcc8ece /src | |
parent | e06263f76f9e38ea9624f1f1ab21af8731a3cd7f (diff) | |
download | px4-firmware-d11235585cf1014c5391b63e394151c171ed286d.tar.gz px4-firmware-d11235585cf1014c5391b63e394151c171ed286d.tar.bz2 px4-firmware-d11235585cf1014c5391b63e394151c171ed286d.zip |
position_estimator_inav: log writing on NaN estimate fixed
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 9 |
1 files 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 fb68dd31c..d6d03367b 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); } |