EKF: Rework nav validity reporting

Remove duplicate checking for dead reckoning and consolidate into a single function.
Use separate timers to check for start of dead reckoning and check when dead reckoning has been performed for too long for the nav solution to be valid.
Allow the timeout for validity reporting to be adjusted externally.
Separate external reporting of dead reckoning from internal checks.

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)

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()

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)

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

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() {

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