ekf2: remove aid src status fusion_enabled flag

msg/EstimatorAidSource1d.msg

@@ -14,7 +14,6 @@ float32 innovation
 float32 innovation_variance
 float32 test_ratio
 
-bool fusion_enabled          # true when measurements are being fused
 bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused
 

msg/EstimatorAidSource2d.msg

@@ -14,7 +14,6 @@ float32[2] innovation
 float32[2] innovation_variance
 float32[2] test_ratio
 
-bool fusion_enabled          # true when measurements are being fused
 bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused
 

msg/EstimatorAidSource3d.msg

@@ -14,7 +14,6 @@ float32[3] innovation
 float32[3] innovation_variance
 float32[3] test_ratio
 
-bool fusion_enabled          # true when measurements are being fused
 bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused
 

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -151,8 +151,6 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
 	aid_src.innovation = innov;
 	aid_src.innovation_variance = innov_var;
 
-	aid_src.fusion_enabled = _control_status.flags.fuse_aspd;
-
 	aid_src.timestamp_sample = airspeed_sample.time_us;
 
 	const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f);

src/modules/ekf2/EKF/auxvel_fusion.cpp

@@ -45,7 +45,6 @@ void Ekf::controlAuxVelFusion()
 			updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
 
 			if (isHorizontalAidingActive()) {
-				_aid_src_aux_vel.fusion_enabled = true;
 				fuseVelocity(_aid_src_aux_vel);
 			}
 		}

src/modules/ekf2/EKF/baro_height_control.cpp

@@ -121,7 +121,6 @@ void Ekf::controlBaroHeightFusion()
 				&& isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL);
 
 		if (_control_status.flags.baro_hgt) {
-			aid_src.fusion_enabled = true;
 
 			if (continuing_conditions_passing) {
 

src/modules/ekf2/EKF/ekf.h

@@ -1241,7 +1241,6 @@ private:
 			status.innovation_variance = 0;
 			status.test_ratio = INFINITY;
 
-			status.fusion_enabled = false;
 			status.innovation_rejected = true;
 			status.fused = false;
 		}
@@ -1265,7 +1264,6 @@ private:
 				status.test_ratio[i] = INFINITY;
 			}
 
-			status.fusion_enabled = false;
 			status.innovation_rejected = true;
 			status.fused = false;
 		}

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -104,10 +104,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 			&& continuing_conditions_passing;
 
 	if (_control_status.flags.ev_hgt) {
-		aid_src.fusion_enabled = true;
-
 		if (continuing_conditions_passing) {
-
 			if (ev_reset) {
 
 				if (quality_sufficient) {

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -175,7 +175,6 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 			&& continuing_conditions_passing;
 
 	if (_control_status.flags.ev_pos) {
-		aid_src.fusion_enabled = true;
 
 		if (continuing_conditions_passing) {
 			const bool bias_estimator_change = (bias_fusion_was_active != _ev_pos_b_est.fusionActive());

src/modules/ekf2/EKF/ev_vel_control.cpp

@@ -138,7 +138,6 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 			&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
 
 	if (_control_status.flags.ev_vel) {
-		aid_src.fusion_enabled = true;
 
 		if (continuing_conditions_passing) {
 

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -76,8 +76,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 			&& isTimedOut(aid_src.time_last_fuse, (uint32_t)1e6);
 
 	if (_control_status.flags.ev_yaw) {
-		aid_src.fusion_enabled = true;
-
 		if (continuing_conditions_passing) {
 
 			if (ev_reset) {

src/modules/ekf2/EKF/fake_height_control.cpp

@@ -63,9 +63,9 @@ void Ekf::controlFakeHgtFusion()
 			if (continuing_conditions_passing) {
 
 				// always protect against extreme values that could result in a NaN
-				aid_src.fusion_enabled = aid_src.test_ratio < sq(100.0f / innov_gate);
+				if (aid_src.test_ratio < sq(100.0f / innov_gate)) {
-
+					fuseVerticalPosition(aid_src);
-				fuseVerticalPosition(aid_src);
+				}
 
 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);
 

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -76,10 +76,11 @@ void Ekf::controlFakePosFusion()
 			if (continuing_conditions_passing) {
 
 				// always protect against extreme values that could result in a NaN
-				aid_src.fusion_enabled = (aid_src.test_ratio[0] < sq(100.0f / innov_gate))
+				if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate))
-							 && (aid_src.test_ratio[1] < sq(100.0f / innov_gate));
+				&& (aid_src.test_ratio[1] < sq(100.0f / innov_gate))
-
+				) {
-				fuseHorizontalPosition(aid_src);
+					fuseHorizontalPosition(aid_src);
+				}
 
 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);
 

src/modules/ekf2/EKF/gnss_height_control.cpp

@@ -98,8 +98,6 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 				&& !gps_checks_failing;
 
 		if (_control_status.flags.gps_hgt) {
-			aid_src.fusion_enabled = true;
-
 			if (continuing_conditions_passing) {
 
 				fuseVerticalPosition(aid_src);

src/modules/ekf2/EKF/gps_control.cpp

@@ -95,7 +95,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 					   vel_obs_var,                                                // observation variance
 					   math::max(_params.gps_vel_innov_gate, 1.f),                 // innovation gate
 					   _aid_src_gnss_vel);
-		_aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
+		const bool gnss_vel_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
 
 		// GNSS position
 		const Vector2f position{gps_sample.pos};
@@ -117,13 +117,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 						     pos_obs_var,                                // observation variance
 						     math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
 						     _aid_src_gnss_pos);
-		_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
+		const bool gnss_pos_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
 
 		// Determine if we should use GPS aiding for velocity and horizontal position
 		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
 		bool mandatory_conditions_passing = false;
 
-		if (((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))
+		if ((gnss_pos_enabled || gnss_vel_enabled)
 		    && _control_status.flags.tilt_align
 		    && _NED_origin_initialised
 		   ) {
@@ -148,8 +148,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 				if (continuing_conditions_passing
 				    || !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
 
-					fuseVelocity(_aid_src_gnss_vel);
+					if (gnss_vel_enabled) {
-					fuseHorizontalPosition(_aid_src_gnss_pos);
+						fuseVelocity(_aid_src_gnss_vel);
+					}
+
+					if (gnss_pos_enabled) {
+						fuseHorizontalPosition(_aid_src_gnss_pos);
+					}
 
 					bool do_vel_pos_reset = shouldResetGpsFusion();
 
@@ -196,15 +201,19 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 					if (do_vel_pos_reset) {
 						ECL_WARN("GPS fusion timeout, resetting velocity and position");
 
-						// reset velocity
+						if (gnss_vel_enabled) {
-						_information_events.flags.reset_vel_to_gps = true;
+							// reset velocity
-						resetVelocityTo(velocity, vel_obs_var);
+							_information_events.flags.reset_vel_to_gps = true;
-						_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
+							resetVelocityTo(velocity, vel_obs_var);
+							_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
+						}
 
-						// reset position
+						if (gnss_pos_enabled) {
-						_information_events.flags.reset_pos_to_gps = true;
+							// reset position
-						resetHorizontalPositionTo(position, pos_obs_var);
+							_information_events.flags.reset_pos_to_gps = true;
-						_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
+							resetHorizontalPositionTo(position, pos_obs_var);
+							_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
+						}
 					}
 
 				} else {
@@ -228,15 +237,19 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 				    || !_control_status_prev.flags.yaw_align
 				   ) {
 					// reset velocity
-					_information_events.flags.reset_vel_to_gps = true;
+					if (gnss_vel_enabled) {
-					resetVelocityTo(velocity, vel_obs_var);
+						_information_events.flags.reset_vel_to_gps = true;
-					_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
+						resetVelocityTo(velocity, vel_obs_var);
+						_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
+					}
 				}
 
-				// reset position
+				if (gnss_pos_enabled) {
-				_information_events.flags.reset_pos_to_gps = true;
+					// reset position
-				resetHorizontalPositionTo(position, pos_obs_var);
+					_information_events.flags.reset_pos_to_gps = true;
-				_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
+					resetHorizontalPositionTo(position, pos_obs_var);
+					_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
+				}
 
 				_control_status.flags.gps = true;
 			}

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -73,8 +73,6 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample)
 		gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg);
 		gnss_yaw.innovation_variance = heading_innov_var;
 
-		gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw;
-
 		gnss_yaw.timestamp_sample = gps_sample.time_us;
 
 		const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);

src/modules/ekf2/EKF/gravity_fusion.cpp

@@ -80,11 +80,9 @@ void Ekf::controlGravityFusion(const imuSample &imu)
 	float innovation_gate = 1.f;
 	setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate);
 
-	_aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector;
-
 	const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2];
 
-	if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
+	if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
 		// perform fusion for each axis
 		_aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0))
 					 && measurementUpdate(Ky, innovation_variance(1), innovation(1))

src/modules/ekf2/EKF/mag_3d_control.cpp

@@ -89,7 +89,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 
 	if (_control_status.flags.mag) {
 		aid_src.timestamp_sample = mag_sample.time_us;
-		aid_src.fusion_enabled = true;
 
 		if (continuing_conditions_passing && _control_status.flags.yaw_align) {
 
@@ -190,8 +189,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 			_nb_mag_3d_reset_available = 2;
 		}
 	}
-
-	aid_src.fusion_enabled = _control_status.flags.mag;
 }
 
 void Ekf::stopMagFusion()

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -133,8 +133,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 		aid_src_mag.innovation[i] = mag_innov(i);
 	}
 
-	aid_src_mag.fusion_enabled = _control_status.flags.mag;
-
 	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
 	if (_control_status.flags.synthetic_mag_z) {
 		aid_src_mag.innovation[2] = 0.0f;

src/modules/ekf2/EKF/mag_heading_control.cpp

@@ -97,7 +97,6 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common
 			&& isTimedOut(aid_src.time_last_fuse, 3e6);
 
 	if (_control_status.flags.mag_hdg) {
-		aid_src.fusion_enabled = true;
 		aid_src.timestamp_sample = mag_sample.time_us;
 
 		if (continuing_conditions_passing && _control_status.flags.yaw_align) {
@@ -185,8 +184,6 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common
 		}
 	}
 
-	aid_src.fusion_enabled = _control_status.flags.mag_hdg;
-
 	// record corresponding mag heading and yaw state for future mag heading delta heading innovation (logging only)
 	_mag_heading_prev = measured_hdg;
 	_mag_heading_pred_prev = getEulerYaw(_state.quat_nominal);

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -90,8 +90,6 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 
 void Ekf::fuseOptFlow()
 {
-	_aid_src_optical_flow.fusion_enabled = true;
-
 	const float R_LOS = _aid_src_optical_flow.observation_variance[0];
 
 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED

src/modules/ekf2/EKF/range_height_control.cpp

@@ -131,8 +131,6 @@ void Ekf::controlRangeHeightFusion()
 				&& _range_sensor.isRegularlySendingData();
 
 		if (_control_status.flags.rng_hgt) {
-			aid_src.fusion_enabled = true;
-
 			if (continuing_conditions_passing) {
 
 				fuseVerticalPosition(aid_src);

src/modules/ekf2/EKF/sideslip_fusion.cpp

@@ -94,8 +94,6 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const
 	sideslip.innovation = innov;
 	sideslip.innovation_variance = innov_var;
 
-	sideslip.fusion_enabled = _control_status.flags.fuse_aspd;
-
 	sideslip.timestamp_sample = _time_delayed_us;
 
 	const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f);

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -176,8 +176,6 @@ void Ekf::controlHaglRngFusion()
 		// No data anymore. Stop until it comes back.
 		stopHaglRngFusion();
 	}
-
-	_aid_src_terrain_range_finder.fusion_enabled = _hagl_sensor_status.flags.range_finder;
 }
 
 float Ekf::getRngVar() const
@@ -241,7 +239,6 @@ void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const
 
 	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
 
-	aid_src.fusion_enabled = false;
 	aid_src.fused = false;
 }
 
@@ -319,8 +316,6 @@ void Ekf::controlHaglFlowFusion()
 		// No data anymore. Stop until it comes back.
 		stopHaglFlowFusion();
 	}
-
-	_aid_src_terrain_optical_flow.fusion_enabled = _hagl_sensor_status.flags.flow;
 }
 
 void Ekf::stopHaglFlowFusion()
@@ -345,7 +340,6 @@ void Ekf::resetHaglFlow()
 
 void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
 {
-	flow.fusion_enabled = true;
 	flow.fused = true;
 
 	const float R_LOS = flow.observation_variance[0];

src/modules/ekf2/EKF/vel_pos_fusion.cpp

@@ -134,7 +134,7 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve
 
 void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
 {
-	if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
+	if (!aid_src.innovation_rejected) {
 		// vx, vy
 		if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
 		    && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
@@ -150,7 +150,7 @@ void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
 
 void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
 {
-	if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
+	if (!aid_src.innovation_rejected) {
 		// vx, vy, vz
 		if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
 		    && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
@@ -168,7 +168,7 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
 void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
 {
 	// x & y
-	if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
+	if (!aid_src.innovation_rejected) {
 		if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx)
 		    && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
 		   ) {
@@ -184,7 +184,7 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
 void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
 {
 	// z
-	if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
+	if (!aid_src.innovation_rejected) {
 		if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) {
 			aid_src.fused = true;
 			aid_src.time_last_fuse = _time_delayed_us;

src/modules/ekf2/EKF/yaw_fusion.cpp

@@ -55,80 +55,77 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 	// innovation test ratio
 	setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
 
-	if (aid_src_status.fusion_enabled) {
+	// check if the innovation variance calculation is badly conditioned
+	if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
+		// the innovation variance contribution from the state covariances is not negative, no fault
+		_fault_status.flags.bad_hdg = false;
 
-		// check if the innovation variance calculation is badly conditioned
+	} else {
-		if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
+		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
-			// the innovation variance contribution from the state covariances is not negative, no fault
+		_fault_status.flags.bad_hdg = true;
-			_fault_status.flags.bad_hdg = false;
+
+		// we reinitialise the covariance matrix and abort this fusion step
+		initialiseCovariance();
+		ECL_ERR("yaw fusion numerical error - covariance reset");
+
+		return false;
+	}
+
+	// calculate the Kalman gains
+	// only calculate gains for states we are using
+	VectorState Kfusion;
+	const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
+
+	for (uint8_t row = 0; row < State::size; row++) {
+		for (uint8_t col = 0; col <= 3; col++) {
+			Kfusion(row) += P(row, col) * H_YAW(col);
+		}
+
+		Kfusion(row) *= heading_innov_var_inv;
+	}
+
+	// set the magnetometer unhealthy if the test fails
+	if (aid_src_status.innovation_rejected) {
+		_innov_check_fail_status.flags.reject_yaw = true;
+
+		// if we are in air we don't want to fuse the measurement
+		// we allow to use it when on the ground because the large innovation could be caused
+		// by interference or a large initial gyro bias
+		if (!_control_status.flags.in_air
+			&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
+			&& isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
+			) {
+			// constrain the innovation to the maximum set by the gate
+			// we need to delay this forced fusion to avoid starting it
+			// immediately after touchdown, when the drone is still armed
+			float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
+			aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
+
+			// also reset the yaw gyro variance to converge faster and avoid
+			// being stuck on a previous bad estimate
+			resetGyroBiasZCov();
 
 		} else {
-			// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
-			_fault_status.flags.bad_hdg = true;
-
-			// we reinitialise the covariance matrix and abort this fusion step
-			initialiseCovariance();
-			ECL_ERR("yaw fusion numerical error - covariance reset");
-
 			return false;
 		}
 
-		// calculate the Kalman gains
+	} else {
-		// only calculate gains for states we are using
+		_innov_check_fail_status.flags.reject_yaw = false;
-		VectorState Kfusion;
+	}
-		const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
 
-		for (uint8_t row = 0; row < State::size; row++) {
+	if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
-			for (uint8_t col = 0; col <= 3; col++) {
-				Kfusion(row) += P(row, col) * H_YAW(col);
-			}
 
-			Kfusion(row) *= heading_innov_var_inv;
+		_time_last_heading_fuse = _time_delayed_us;
-		}
 
-		// set the magnetometer unhealthy if the test fails
+		aid_src_status.time_last_fuse = _time_delayed_us;
-		if (aid_src_status.innovation_rejected) {
+		aid_src_status.fused = true;
-			_innov_check_fail_status.flags.reject_yaw = true;
 
-			// if we are in air we don't want to fuse the measurement
+		_fault_status.flags.bad_hdg = false;
-			// we allow to use it when on the ground because the large innovation could be caused
-			// by interference or a large initial gyro bias
-			if (!_control_status.flags.in_air
-			    && isTimedOut(_time_last_in_air, (uint64_t)5e6)
-			    && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
-			   ) {
-				// constrain the innovation to the maximum set by the gate
-				// we need to delay this forced fusion to avoid starting it
-				// immediately after touchdown, when the drone is still armed
-				float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
-				aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
 
-				// also reset the yaw gyro variance to converge faster and avoid
+		return true;
-				// being stuck on a previous bad estimate
-				resetGyroBiasZCov();
 
-			} else {
+	} else {
-				return false;
+		_fault_status.flags.bad_hdg = true;
-			}
-
-		} else {
-			_innov_check_fail_status.flags.reject_yaw = false;
-		}
-
-		if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
-
-			_time_last_heading_fuse = _time_delayed_us;
-
-			aid_src_status.time_last_fuse = _time_delayed_us;
-			aid_src_status.fused = true;
-
-			_fault_status.flags.bad_hdg = false;
-
-			return true;
-
-		} else {
-			_fault_status.flags.bad_hdg = true;
-		}
 	}
 
 	// otherwise

src/modules/ekf2/EKF/zero_innovation_heading_update.cpp

@@ -62,7 +62,6 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 
 		if (!_control_status.flags.tilt_align || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
 			// The yaw variance is too large, fuse fake measurement
-			aid_src_status.fusion_enabled = true;
 			fuseYaw(aid_src_status, H_YAW);
 		}
 	}