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

msg/estimator_status_flags.msg

@@ -33,6 +33,8 @@ bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measur
 bool cs_vehicle_at_rest       # 26 - true when the vehicle is at rest
 bool cs_gps_yaw_fault         # 27 - true when the GNSS heading has been declared faulty and is no longer being used
 bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
+bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
+bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

msg/vehicle_status_flags.msg

@@ -13,12 +13,19 @@ bool condition_local_altitude_valid
 bool condition_local_position_valid		# set to true by the commander app if the quality of the local position estimate is good enough to use for navigation
 bool condition_local_velocity_valid		# set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation
 bool condition_global_position_valid		# set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
+bool condition_gps_position_valid
 bool condition_home_position_valid		# indicates a valid home position (a valid home position is not always a valid launch)
 bool condition_power_input_valid                # set if input power is valid
 bool condition_battery_healthy                # set if battery is available and not low
 bool condition_escs_error		      # set to true if one or more ESCs reporting esc_status are offline
 bool condition_escs_failure		      # set to true if one or more ESCs reporting esc_status has a failure
 
+bool position_reliant_on_gps
+bool position_reliant_on_optical_flow
+bool position_reliant_on_vision_position
+
+bool dead_reckoning
+
 bool circuit_breaker_engaged_power_check
 bool circuit_breaker_engaged_airspd_check
 bool circuit_breaker_engaged_enginefailure_check

src/lib/events/enums.json

@@ -110,6 +110,10 @@
                         "6": {
                             "name": "no_rc_and_no_datalink",
                             "description": "No RC and no datalink"
+                        },
+                        "7": {
+                            "name": "no_gps",
+                            "description": "No valid GPS"
                         }
                     }
                 },

src/modules/commander/Commander.cpp

@@ -811,6 +811,9 @@ Commander::Commander() :
 
 	// default for vtol is rotary wing
 	_vtol_status.vtol_in_rw_mode = true;
+
+	_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
+	_vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME);
 }
 
 bool
@@ -3983,9 +3986,8 @@ void Commander::estimator_check()
 		_heading_reset_counter = lpos.heading_reset_counter;
 	}
 
-	const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
-	const bool gnss_heading_fault_prev = (_estimator_status_sub.get().control_mode_flags &
-					      (1 << estimator_status_s::CS_GPS_YAW_FAULT));
+	const bool mag_fault_prev = _estimator_status_flags_sub.get().cs_mag_fault;
+	const bool gnss_heading_fault_prev = _estimator_status_flags_sub.get().cs_gps_yaw_fault;
 
 	// use primary estimator_status
 	if (_estimator_selector_status_sub.updated()) {
@@ -3994,41 +3996,58 @@ void Commander::estimator_check()
 		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
 			if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) {
 				_estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance);
+				_estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance);
 			}
 		}
 	}
 
-	if (_estimator_status_sub.update()) {
-		const estimator_status_s &estimator_status = _estimator_status_sub.get();
+	if (_estimator_status_flags_sub.update()) {
+		const estimator_status_flags_s &estimator_status_flags = _estimator_status_flags_sub.get();
 
-		// Check for a magnetomer fault and notify the user
-		const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
-		const bool gnss_heading_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS_YAW_FAULT));
+		_status_flags.dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
+					       || estimator_status_flags.cs_inertial_dead_reckoning;
 
-		if (!mag_fault_prev && mag_fault) {
+		if (!(estimator_status_flags.cs_inertial_dead_reckoning || estimator_status_flags.cs_wind_dead_reckoning)) {
+			// position requirements (update if not dead reckoning)
+			bool gps             = estimator_status_flags.cs_gps;
+			bool optical_flow    = estimator_status_flags.cs_opt_flow;
+			bool vision_position = estimator_status_flags.cs_ev_pos;
+
+			_status_flags.position_reliant_on_gps             =  gps && !optical_flow && !vision_position;
+			_status_flags.position_reliant_on_optical_flow    = !gps &&  optical_flow && !vision_position;
+			_status_flags.position_reliant_on_vision_position = !gps && !optical_flow &&  vision_position;
+		}
+
+		// Check for a magnetomer fault and notify the user
+		if (!mag_fault_prev && estimator_status_flags.cs_mag_fault) {
 			mavlink_log_critical(&_mavlink_log_pub, "Compass needs calibration - Land now!\t");
 			events::send(events::ID("commander_stopping_mag_use"), events::Log::Critical,
 				     "Stopping compass use! Land now and calibrate the compass");
 			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _status);
 		}
 
-		if (!gnss_heading_fault_prev && gnss_heading_fault) {
+		if (!gnss_heading_fault_prev && estimator_status_flags.cs_gps_yaw_fault) {
 			mavlink_log_critical(&_mavlink_log_pub, "GNSS heading not reliable - Land now!\t");
 			events::send(events::ID("commander_stopping_gnss_heading_use"), events::Log::Critical,
 				     "GNSS heading not reliable. Land now!");
 			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, false, _status);
 		}
+	}
 
-		/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
-		 * for a short time interval after takeoff.
-		 * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
-		 * heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
-		 * if this does not fix the issue we need to stop using a position controlled
-		 * mode to prevent flyaway crashes.
-		 */
 
-		if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+	/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
+	* for a short time interval after takeoff.
+	* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
+	* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
+	* if this does not fix the issue we need to stop using a position controlled
+	* mode to prevent flyaway crashes.
+	*/
+	if (_estimator_status_sub.updated() && run_quality_checks
+	    && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+
+		estimator_status_s estimator_status;
 
+		if (_estimator_status_sub.copy(&estimator_status)) {
 			if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
 				_nav_test_failed = false;
 				_nav_test_passed = false;
@@ -4036,9 +4055,10 @@ void Commander::estimator_check()
 			} else {
 				if (!_nav_test_passed) {
 					// Both test ratios need to pass/fail together to change the nav test status
-					const bool innovation_pass = (estimator_status.vel_test_ratio < 1.0f) && (estimator_status.pos_test_ratio < 1.0f)
+					const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
 								     && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
-					const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.0f) && (estimator_status.pos_test_ratio >= 1.0f);
+
+					const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
 
 					if (innovation_pass) {
 						_time_last_innov_pass = hrt_absolute_time();
@@ -4122,13 +4142,47 @@ void Commander::estimator_check()
 	}
 
 	_status_flags.condition_angular_velocity_valid = condition_angular_velocity_valid;
+
+
+	// gps
+	const bool condition_gps_position_was_valid = _status_flags.condition_gps_position_valid;
+
+	if (_vehicle_gps_position_sub.updated()) {
+		vehicle_gps_position_s vehicle_gps_position;
+
+		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) {
+
+			bool time = (vehicle_gps_position.timestamp != 0) && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s);
+
+			bool fix = vehicle_gps_position.fix_type >= 2;
+			bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get();
+			bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get();
+			bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get();
+
+			_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh, hrt_absolute_time());
+			_status_flags.condition_gps_position_valid = _vehicle_gps_position_valid.get_state();
+
+			_vehicle_gps_position_timestamp_last = vehicle_gps_position.timestamp;
+		}
+
+	} else {
+		const hrt_abstime now_us = hrt_absolute_time();
+
+		if (now_us > _vehicle_gps_position_timestamp_last + GPS_VALID_TIME) {
+			_vehicle_gps_position_valid.set_state_and_update(false, now_us);
+			_status_flags.condition_gps_position_valid = false;
+		}
+	}
+
+	if (condition_gps_position_was_valid && !_status_flags.condition_gps_position_valid) {
+		PX4_WARN("GPS no longer valid");
+	}
 }
 
 void Commander::UpdateEstimateValidity()
 {
 	const vehicle_local_position_s &lpos = _local_position_sub.get();
 	const vehicle_global_position_s &gpos = _global_position_sub.get();
-	const estimator_status_s &status = _estimator_status_sub.get();
 
 	float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get();
 
@@ -4139,11 +4193,7 @@ void Commander::UpdateEstimateValidity()
 
 		// Set the allowable position uncertainty based on combination of flight and estimator state
 		// When we are in a operator demanded position control mode and are solely reliant on optical flow, do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
-		const bool reliant_on_opt_flow = ((status.control_mode_flags & (1 << estimator_status_s::CS_OPT_FLOW))
-						  && !(status.control_mode_flags & (1 << estimator_status_s::CS_GPS))
-						  && !(status.control_mode_flags & (1 << estimator_status_s::CS_EV_POS)));
-
-		if (reliant_on_opt_flow) {
+		if (_status_flags.position_reliant_on_optical_flow) {
 			lpos_eph_threshold_adj = INFINITY;
 		}
 	}

src/modules/commander/Commander.hpp

@@ -69,6 +69,7 @@
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/estimator_status_flags.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -84,6 +85,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
@@ -314,6 +316,10 @@ private:
 	bool		_nav_test_passed{false};	/**< true if the post takeoff navigation test has passed */
 	bool		_nav_test_failed{false};	/**< true if the post takeoff navigation test has failed */
 
+	static constexpr hrt_abstime GPS_VALID_TIME{3_s};
+	Hysteresis _vehicle_gps_position_valid{false};
+	hrt_abstime _vehicle_gps_position_timestamp_last{0};
+
 	bool		_geofence_loiter_on{false};
 	bool		_geofence_rtl_on{false};
 	bool		_geofence_land_on{false};
@@ -411,6 +417,7 @@ private:
 	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
 	uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
 	uORB::Subscription					_estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
+	uORB::Subscription					_estimator_status_sub{ORB_ID(estimator_status)};
 	uORB::Subscription					_geofence_result_sub{ORB_ID(geofence_result)};
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 	uORB::Subscription					_land_detector_sub{ORB_ID(vehicle_land_detected)};
@@ -420,6 +427,7 @@ private:
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription					_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription					_vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
 	uORB::Subscription					_wind_sub{ORB_ID(wind)};
 
@@ -433,8 +441,7 @@ private:
 	uORB::Subscription					_power_button_state_sub {ORB_ID(power_button_state)};
 #endif // BOARD_HAS_POWER_CONTROL
 
-	uORB::SubscriptionData<airspeed_s>			_airspeed_sub{ORB_ID(airspeed)};
-	uORB::SubscriptionData<estimator_status_s>		_estimator_status_sub{ORB_ID(estimator_status)};
+	uORB::SubscriptionData<estimator_status_flags_s>	_estimator_status_flags_sub{ORB_ID(estimator_status_flags)};
 	uORB::SubscriptionData<mission_result_s>		_mission_result_sub{ORB_ID(mission_result)};
 	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};

src/modules/commander/state_machine_helper.cpp

@@ -59,6 +59,7 @@ static constexpr const char reason_no_local_position[] = "no local position";
 static constexpr const char reason_no_global_position[] = "no global position";
 static constexpr const char reason_no_datalink[] = "no datalink";
 static constexpr const char reason_no_rc_and_no_datalink[] = "no RC and no datalink";
+static constexpr const char reason_no_gps[] = "no GPS";
 
 // This array defines the arming state transitions. The rows are the new state, and the columns
 // are the current state. Using new state and current state you can index into the array which
@@ -476,6 +477,8 @@ static void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_adv
 		case event_failsafe_reason_t::no_datalink: reason = reason_no_datalink; break;
 
 		case event_failsafe_reason_t::no_rc_and_no_datalink: reason = reason_no_rc_and_no_datalink; break;
+
+		case event_failsafe_reason_t::no_gps: reason = reason_no_gps; break;
 		}
 
 		mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s\t", reason);

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;

src/modules/ekf2/EKF2.cpp

@@ -744,7 +744,8 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 			global_pos.terrain_alt_valid = false;
 		}
 
-		global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning
+		global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
+					    || _ekf.control_status_flags().wind_dead_reckoning;
 		global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_global_position_pub.publish(global_pos);
 	}
@@ -1273,6 +1274,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_vehicle_at_rest       = _ekf.control_status_flags().vehicle_at_rest;
 		status_flags.cs_gps_yaw_fault         = _ekf.control_status_flags().gps_yaw_fault;
 		status_flags.cs_rng_fault             = _ekf.control_status_flags().rng_fault;
+		status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
+		status_flags.cs_wind_dead_reckoning     = _ekf.control_status_flags().wind_dead_reckoning;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;