commander: monitor GPS validity and EKF2 dead reckoning

- ekf2: expose dead reckoning as control status flags
 - commander:
    - add GPS validity check
    - in AUTO MISSION if dependent on GPS then a loss of GPS will

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -58,7 +58,7 @@ void Ekf::fuseAirspeed()
 			       math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
 
 	// determine if we need the airspeed fusion to correct states other than wind
-	const bool update_wind_only = !_is_wind_dead_reckoning;
+	const bool update_wind_only = !_control_status.flags.wind_dead_reckoning;
 
 	// Intermediate variables
 	const float HK0 = vn - vwn;

src/modules/ekf2/EKF/common.h

@@ -491,6 +491,8 @@ union filter_control_status_u {
 		uint32_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest
 		uint32_t gps_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
 		uint32_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
+		uint32_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
+		uint32_t wind_dead_reckoning     : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/control.cpp

@@ -422,7 +422,7 @@ void Ekf::controlOpticalFlowFusion()
 
 		// Check if we are in-air and require optical flow to control position drift
 		const bool is_flow_required = _control_status.flags.in_air
-					      && (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
+					      && (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
 						  || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)
 						  || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -739,11 +739,14 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
-	if (_is_dead_reckoning && (_control_status.flags.gps)) {
-		hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
+	if (_control_status.flags.inertial_dead_reckoning) {
+		if (_control_status.flags.gps) {
+			hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
+		}
 
-	} else if (_is_dead_reckoning && (_control_status.flags.ev_pos)) {
-		hpos_err = math::max(hpos_err, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1))));
+		if (_control_status.flags.ev_pos) {
+			hpos_err = math::max(hpos_err, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1))));
+		}
 	}
 
 	*ekf_eph = hpos_err;
@@ -1017,10 +1020,10 @@ void Ekf::update_deadreckoning_status()
 				   isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) &&
 				   isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max);
 
-	_is_wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
-	_is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
+	_control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
+	_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
 
-	if (!_is_dead_reckoning) {
+	if (!_control_status.flags.inertial_dead_reckoning) {
 		_time_last_aiding = _time_last_imu - _params.no_aid_timeout_max;
 	}
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -180,9 +180,6 @@ public:
 
 	int getNumberOfActiveHorizontalAidingSources() const;
 
-	// return true if the EKF is dead reckoning the position using inertial data only
-	bool inertial_dead_reckoning() const { return _is_dead_reckoning; }
-
 	const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
 
 	// get the velocity of the body frame origin in local NED earth frame
@@ -345,9 +342,7 @@ protected:
 	Vector2f _drag_test_ratio{};		// drag innovation consistency check ratio
 	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{true};	// true if the horizontal nav solution has been deadreckoning for too long and is invalid
-	bool _is_wind_dead_reckoning{false};	// true if we are navigationg reliant on wind relative measurements
 
 	float _gps_horizontal_position_drift_rate_m_s{0}; // Horizontal position drift rate (m/s)
 	float _gps_vertical_position_drift_rate_m_s{0};   // Vertical position drift rate (m/s)

src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp

@@ -138,7 +138,7 @@ int main()
         const float HK52 = HK16/_beta_innov_var;
 
         // determine if we need the sideslip fusion to correct states other than wind
-        // bool update_wind_only = !_is_wind_dead_reckoning;
+        // bool update_wind_only = !_control_status.flags.wind_dead_reckoning;
         bool update_wind_only = false;
 
         // // Calculate predicted sideslip angle and innovation using small angle approximation

src/modules/ekf2/EKF/sideslip_fusion.cpp

@@ -72,7 +72,7 @@ void Ekf::fuseSideslip()
 		const float R_BETA = sq(_params.beta_noise); // observation noise variance
 
 		// determine if we need the sideslip fusion to correct states other than wind
-		bool update_wind_only = !_is_wind_dead_reckoning;
+		bool update_wind_only = !_control_status.flags.wind_dead_reckoning;
 
 		// Intermediate Values
 		const float HK0 = vn - vwn;