|
|
|
@ -167,12 +167,13 @@ void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3],
@@ -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); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -708,6 +709,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
@@ -708,6 +709,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 +721,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
@@ -715,7 +721,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 +746,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
@@ -739,7 +746,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)); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|