diff --git a/EKF/common.h b/EKF/common.h index cb3b26210e..45e376615e 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -326,6 +326,8 @@ struct parameters { ///< the EKF will report that it has been inertial dead-reckoning for too long and needs to revert to a /// mode that doesn't privide horizontal vbelocity and position estimates (uSec) + int32_t valid_timeout_max{5000000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec) + // multi-rotor drag specific force fusion float drag_noise{2.5f}; ///< observation noise variance for drag specific force measurements (m/sec**2)**2 float bcoef_x{25.0f}; ///< ballistic coefficient along the X-axis (kg/m**2) diff --git a/EKF/control.cpp b/EKF/control.cpp index 039899c769..3e0300f7d4 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -136,12 +136,8 @@ void Ekf::controlFusionModes() // Additional NE velocity data from an auxiliary sensor can be fused controlAuxVelFusion(); - // report dead reckoning if we are no longer fusing measurements that directly constrain velocity drift - _is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > _params.no_aid_timeout_max) - && (_time_last_imu - _time_last_delpos_fuse > _params.no_aid_timeout_max) - && (_time_last_imu - _time_last_vel_fuse > _params.no_aid_timeout_max) - && (_time_last_imu - _time_last_of_fuse > _params.no_aid_timeout_max); - + // check if we are no longer fusing measurements that directly constrain velocity drift + update_deadreckoning_status(); } void Ekf::controlExternalVisionFusion() diff --git a/EKF/ekf.h b/EKF/ekf.h index a88df3018c..07896a4df0 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -173,8 +173,8 @@ public: // return true if the global position estimate is valid bool global_position_is_valid(); - // return true if the EKF is dead reckoning the position using inertial data only - bool inertial_dead_reckoning(); + // check if the EKF is dead reckoning horizontal velocity using inertial data only + void update_deadreckoning_status(); // return true if the terrain estimate is valid bool get_terrain_valid(); @@ -291,6 +291,7 @@ private: bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused uint64_t _time_last_fake_gps{0}; ///< last time we faked GPS position measurements to constrain tilt errors during operation without external aiding (uSec) + uint64_t _time_ins_deadreckon_start{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) uint64_t _time_last_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec) uint64_t _time_last_delpos_fuse{0}; ///< time the last fusion of incremental horizontal position measurements was performed (uSec) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index e4c1556bc0..7e6fc15676 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1296,11 +1296,11 @@ void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, bool Ekf::global_position_is_valid() { // return true if we are not doing unconstrained free inertial navigation and the origin is set - return (_NED_origin_initialised && !inertial_dead_reckoning()); + return (_NED_origin_initialised && !_deadreckon_time_exceeded); } // return true if we are totally reliant on inertial dead-reckoning for position -bool Ekf::inertial_dead_reckoning() +void Ekf::update_deadreckoning_status() { bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos) && ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max) @@ -1309,7 +1309,15 @@ bool Ekf::inertial_dead_reckoning() bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= _params.no_aid_timeout_max); bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= _params.no_aid_timeout_max) && (_time_last_imu - _time_last_beta_fuse <= _params.no_aid_timeout_max); - return !velPosAiding && !optFlowAiding && !airDataAiding; + _is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; + + // record the time we start inertial dead reckoning + if (!_is_dead_reckoning) { + _time_ins_deadreckon_start = _time_last_imu - _params.no_aid_timeout_max; + } + + // report if we have been deadreckoning for too long + _deadreckon_time_exceeded = ((_time_last_imu - _time_ins_deadreckon_start) > (unsigned)_params.valid_timeout_max); } // perform a vector cross product diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 5dbbec9108..b79d79e179 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -550,7 +550,7 @@ void EstimatorInterface::unallocate_buffers() bool EstimatorInterface::local_position_is_valid() { // return true if we are not doing unconstrained free inertial navigation - return !inertial_dead_reckoning(); + return !_deadreckon_time_exceeded; } void EstimatorInterface::print_status() { diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 26f119d9f5..76cd703407 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -237,7 +237,7 @@ public: virtual bool global_position_is_valid() = 0; // return true if the EKF is dead reckoning the position using inertial data only - virtual bool inertial_dead_reckoning() = 0; + bool inertial_dead_reckoning() {return _is_dead_reckoning;} // return true if the terrain estimate is valid virtual bool get_terrain_valid() = 0; @@ -453,6 +453,7 @@ protected: innovation_fault_status_u _innov_check_fail_status{}; bool _is_dead_reckoning{false}; // true if we are no longer fusing measurements that constrain horizontal velocity drift + bool _deadreckon_time_exceeded{false}; // true if the horizontal nav solution has been deadreckoning for too long and is invalid // IMU vibration monitoring Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement