From e10ec590587ca0f590b0adc5f085b3289b051b37 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Oct 2017 11:16:48 +1100 Subject: [PATCH] EKF: Use consistent test for navigation validity reporting This will enable controller to take advantage of non-inertial dead reckoning. --- EKF/ekf_helper.cpp | 5 ++--- EKF/estimator_interface.cpp | 6 ++---- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index f9305acb61..de27c0db40 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1144,9 +1144,8 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first bool Ekf::global_position_is_valid() { - // return true if the position estimate is valid - // TODO implement proper check based on published GPS accuracy, innovation consistency checks and timeout status - return (_NED_origin_initialised && ((_time_last_imu - _time_last_gps) < 5e6) && _control_status.flags.gps); + // return true if we are not doing unconstrained free inertial navigation and the origin is set + return (_NED_origin_initialised && !inertial_dead_reckoning()); } // return true if we are totally reliant on inertial dead-reckoning for position diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 3eedddf3a0..ddee0c0e30 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -464,8 +464,6 @@ 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) && _control_status.flags.opt_flow) || - (((_time_last_imu - _time_last_ext_vision) < 5e6) && _control_status.flags.ev_pos) || - global_position_is_valid(); + // return true if we are not doing unconstrained free inertial navigation + return !inertial_dead_reckoning(); }