EKF: enable gps fusion flag to be false while fusing air data

EKF/common.h

@@ -416,6 +416,7 @@ union filter_control_status_u {
 		uint32_t update_mag_states_only   : 1; ///< 16 - true when only the magnetometer states are updated by the magnetometer
 		uint32_t fixed_wing  : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
 		uint32_t mag_fault   : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used
+		uint32_t fuse_aspd   : 1; ///< 19 - true when airspedd measurements are being fused
 	} flags;
 	uint32_t value;
 };

EKF/control.cpp

@@ -494,14 +494,18 @@ void Ekf::controlGpsFusion()
 	} else {
 		// handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it
 		bool lost_gps = _control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6);
-		bool no_velpos_aiding = !_control_status.flags.opt_flow && !_control_status.flags.ev_pos;
-		bool no_airdata_aiding = (_time_last_imu - _time_last_arsp_fuse > 10e6) || (_time_last_imu - _time_last_beta_fuse > 10e6);
 		// If we don't have a source of aiding to constrain attitude drift, then we need to switch
 		// to the non-aiding mode, zero the velocity states and set the position to the current estimate.
 		// Air data aiding prevents loss of attitude reference, constrains velocity drift and can assist
 		// a FW vehicle to recover to the launch area.
-		if (lost_gps && no_velpos_aiding && no_airdata_aiding) {
+		if (lost_gps) {
 			_control_status.flags.gps = false;
+			ECL_WARN("EKF GPS data stopped");
+
+		}
+		bool no_velpos_aiding = !_control_status.flags.opt_flow && !_control_status.flags.ev_pos;
+		bool no_airdata_aiding = (_time_last_imu - _time_last_arsp_fuse > 10e6) || (_time_last_imu - _time_last_beta_fuse > 10e6);
+		if (!_control_status.flags.gps && no_velpos_aiding && no_airdata_aiding) {
 			_last_known_posNE(0) = _state.pos(0);
 			_last_known_posNE(1) = _state.pos(1);
 			_state.vel.setZero();
@@ -1010,9 +1014,18 @@ void Ekf::controlAirDataFusion()
 
 	}
 
-	// Always try to fuse airspeed data if available and we are in flight and the filter is operating in a normal aiding mode
-	bool is_aiding = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos;
-	if (_tas_data_ready && _control_status.flags.in_air && is_aiding) {
+	if (_control_status.flags.fuse_aspd && airspeed_timed_out) {
+		_control_status.flags.fuse_aspd = false;
+
+	}
+
+	// Always try to fuse airspeed data if available and we are in flight
+	if (_tas_data_ready && _control_status.flags.in_air) {
+		// always fuse airsped data if we are flying and data is present
+		if (!_control_status.flags.fuse_aspd) {
+			_control_status.flags.fuse_aspd = true;
+		}
+
 		// If starting wind state estimation, reset the wind states and covariances before fusing any data
 		if (!_control_status.flags.wind) {
 			// activate the wind states
@@ -1047,10 +1060,7 @@ void Ekf::controlBetaFusion()
 	// Suffient time has lapsed sice the last fusion
 	bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us;
 
-	// The filter is operating in a mode where velocity states can be used
-	bool vel_states_active = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos;
-
-	if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air && vel_states_active) {
+	if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) {
 		// If starting wind state estimation, reset the wind states and covariances before fusing any data
 		if (!_control_status.flags.wind) {
 			// activate the wind states
@@ -1267,8 +1277,11 @@ void Ekf::controlVelPosFusion()
 {
 	// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
 	// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
-	if (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos
-	    && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
+	if (!_control_status.flags.gps &&
+			!_control_status.flags.opt_flow &&
+			!_control_status.flags.ev_pos &&
+			!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) &&
+			((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
 		_fuse_pos = true;
 		_time_last_fake_gps = _time_last_imu;
 

EKF/ekf_helper.cpp

@@ -902,7 +902,10 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
 	// TODO - allow for baro drift in vertical position error
 	float hpos_err;
 	float vpos_err;
-	bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
+	bool vel_pos_aiding = (_control_status.flags.gps ||
+			       _control_status.flags.opt_flow ||
+			       _control_status.flags.ev_pos ||
+			       (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
 
 	if (vel_pos_aiding && _NED_origin_initialised) {
 		hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
@@ -933,7 +936,10 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
 	// TODO - allow for baro drift in vertical position error
 	float hpos_err;
 	float vpos_err;
-	bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
+	bool vel_pos_aiding = (_control_status.flags.gps ||
+			       _control_status.flags.opt_flow ||
+			       _control_status.flags.ev_pos ||
+			       (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
 
 	if (vel_pos_aiding && _NED_origin_initialised) {
 		hpos_err = sqrtf(P[7][7] + P[8][8]);
@@ -963,7 +969,10 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon
 {
 	float hvel_err;
 	float vvel_err;
-	bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
+	bool vel_pos_aiding = (_control_status.flags.gps ||
+			       _control_status.flags.opt_flow ||
+			       _control_status.flags.ev_pos ||
+			       (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
 
 	if (vel_pos_aiding && _NED_origin_initialised) {
 		hvel_err = sqrtf(P[4][4] + P[5][5]);
@@ -1058,7 +1067,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
 {
 	ekf_solution_status soln_status{};
 	soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
-	soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
+	soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
 	soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
 	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow ) && (_fault_status.value == 0);
 	soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
@@ -1146,7 +1155,7 @@ bool Ekf::inertial_dead_reckoning()
 	bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos)
 			    && ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6));
 	bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= 1E6);
-	bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6);
+	bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6) && (_time_last_imu - _time_last_beta_fuse <= 1E6);
 
 	return !velPosAiding && !optFlowAiding && !airDataAiding;
 }

EKF/estimator_interface.h

@@ -199,7 +199,7 @@ public:
 	void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;}
 
 	// set flag if synthetic sideslip measurement should be fused
-	void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = fuse_beta;}
+	void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);}
 
 	// set flag if only only mag states should be updated by the magnetometer
 	void set_update_mag_states_only_flag(bool update_mag_states_only) {_control_status.flags.update_mag_states_only = update_mag_states_only;}