dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

position_estimator_inav: failsafe against NaN estimate

This commit is contained in:
Anton Babushkin 2014-03-01 22:43:56 +04:00
parent 2c337d9b45
commit e06263f76f
1 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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