From eec71d1a10679968da38a64724cba55a8bd47182 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 23 Apr 2018 21:13:52 -0400 Subject: [PATCH] EKF get_ekf_soln_status() fix pred_pos_horiz_abs --- EKF/ekf_helper.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5062c770bd..7f79ef02e7 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1131,7 +1131,8 @@ void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, f // return a bitmask integer that describes which state estimates are valid void Ekf::get_ekf_soln_status(uint16_t *status) { - ekf_solution_status soln_status{}; + ekf_solution_status soln_status; + soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0); soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); @@ -1141,7 +1142,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) soln_status.flags.pos_vert_agl = get_terrain_valid(); soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; - soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_vert_abs; + soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; bool gps_vel_innov_bad = (_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f); bool gps_pos_innov_bad = (_vel_pos_test_ratio[3] > 1.0f) || (_vel_pos_test_ratio[4] > 1.0f); bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f);