diff --git a/EKF/ekf.h b/EKF/ekf.h index e1798260a4..e2df78fdac 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -123,6 +123,9 @@ public: void get_accel_bias(float *bias) {*bias = _state.accel_z_bias;} + // get GPS check status + void get_gps_check_status(uint16_t *_gps_check_fail_status); + private: static const uint8_t _k_num_states = 24; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 929b3b2429..09377f9537 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -316,6 +316,12 @@ void Ekf::get_heading_innov_var(float *heading_innov_var) memcpy(heading_innov_var, &_heading_innov_var, sizeof(float)); } +// get GPS check status +void Ekf::get_gps_check_status(uint16_t *val) +{ + *val = _gps_check_fail_status.value; +} + // get the state vector at the delayed time horizon void Ekf::get_state_delayed(float *state) { diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index ebb7e02d58..a4dfcee8c8 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -366,5 +366,5 @@ void EstimatorInterface::unallocate_buffers() bool EstimatorInterface::local_position_is_valid() { // return true if the position estimate is valid - return ((_time_last_imu - _time_last_optflow) < 5e6) || global_position_is_valid(); + return (((_time_last_imu - _time_last_optflow) < 5e6) && _control_status.flags.opt_flow) || global_position_is_valid(); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 048eca6ce6..b99dafff6d 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -211,6 +211,15 @@ public: virtual void get_accel_bias(float *bias) = 0; + // get EKF mode status + void get_control_mode(uint16_t *val) + { + *val = _control_status.value; + } + + // get GPS check status + virtual void get_gps_check_status(uint16_t *val) = 0; + protected: parameters _params; // filter parameters