diff --git a/EKF/common.h b/EKF/common.h index 4845cda8c4..280a894039 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -445,4 +445,21 @@ union filter_control_status_u { uint16_t value; }; +union ekf_solution_status { + struct { + uint16_t attitude : 1; // 0 - True if the attitude estimate is good + uint16_t velocity_horiz : 1; // 1 - True if the horizontal velocity estimate is good + uint16_t velocity_vert : 1; // 2 - True if the vertical velocity estimate is good + uint16_t pos_horiz_rel : 1; // 3 - True if the horizontal position (relative) estimate is good + uint16_t pos_horiz_abs : 1; // 4 - True if the horizontal position (absolute) estimate is good + uint16_t pos_vert_abs : 1; // 5 - True if the vertical position (absolute) estimate is good + uint16_t pos_vert_agl : 1; // 6 - True if the vertical position (above ground) estimate is good + uint16_t const_pos_mode : 1; // 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) + uint16_t pred_pos_horiz_rel : 1; // 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate + uint16_t pred_pos_horiz_abs : 1; // 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate + uint16_t gps_glitch : 1; // 10 - True if the EKF has detected a GPS glitch + } flags; + uint16_t value; +}; + } diff --git a/EKF/ekf.h b/EKF/ekf.h index ce7380d2af..37437015e7 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -169,6 +169,9 @@ public: // Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned. void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl); + // return a bitmask integer that describes which state estimates can be used for flight control + void get_ekf_soln_status(uint16_t *status); + private: static const uint8_t _k_num_states = 24; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 6d1266dd55..f483a19cb3 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -731,6 +731,28 @@ void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, f *hagl = sqrt(_terr_test_ratio); } +// 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; + 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) && (_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); + soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); + soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); + soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; + float dummy_var; + soln_status.flags.pos_vert_agl = get_terrain_vert_pos(&dummy_var); + 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; + 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); + soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good; + *status = soln_status.value; +} + // fuse measurement void Ekf::fuse(float *K, float innovation) { diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 8b59f1968d..a77cc3d210 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -249,6 +249,8 @@ public: // Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned. virtual void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl) = 0; + // return a bitmask integer that describes which state estimates can be used for flight control + virtual void get_ekf_soln_status(uint16_t *status) = 0; protected: