fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 01:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'inav_fs' into beta_mavlink2

Browse Source
This commit is contained in:
Anton Babushkin 2014-03-01 22:57:30 +04:00
commit cc0f237c16
1 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -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[])
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[])
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[])
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));
}
}