ekf2: merge runOnGroundYawReset() and runInAirYawReset()

src/modules/ekf2/EKF/ekf.h

@@ -724,7 +724,7 @@ private:
 
 	// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
 	// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
-	bool realignYawGPS(const Vector3f &mag);
+	bool realignYawGPS(bool force = false);
 
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();
@@ -844,12 +844,10 @@ private:
 	// control fusion of magnetometer observations
 	void controlMagFusion();
 
-	void checkHaglYawResetReq();
+	bool haglYawResetReq() const;
 	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
 
-	void runOnGroundYawReset();
-	bool canResetMagHeading() const;
-	void runInAirYawReset(const Vector3f &mag);
+	void runYawReset();
 
 	void selectMagAuto();
 	void check3DMagFusionSuitability();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -338,21 +338,25 @@ void Ekf::alignOutputFilter()
 
 // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
 // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle only.
-bool Ekf::realignYawGPS(const Vector3f &mag)
+bool Ekf::realignYawGPS(bool force)
 {
+	if (!_control_status.flags.in_air || !_control_status.flags.fixed_wing) {
+		return false;
+	}
+
 	const float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1)));
 
 	// Need at least 5 m/s of GPS horizontal speed and
 	// ratio of velocity error to velocity < 0.15  for a reliable alignment
-	const bool gps_yaw_alignment_possible = (gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed));
+	const bool gps_yaw_alignment_possible = (gpsSpeed > 5.f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed));
 
 	if (!gps_yaw_alignment_possible) {
 		// attempt a normal alignment using the magnetometer
-		return resetMagHeading();
+		return false;
 	}
 
 	// check for excessive horizontal GPS velocity innovations
-	const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps;
+	const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.f) && _control_status.flags.gps;
 
 	// calculate GPS course over ground angle
 	const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
@@ -364,25 +368,28 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 	const float courseYawError = wrap_pi(gpsCOG - ekfCOG);
 
 	// If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
-	const bool badYawErr = fabsf(courseYawError) > 0.5f;
+	const bool badYawErr = fabsf(courseYawError) > math::radians(25.f);
 	const bool badMagYaw = (badYawErr && badVelInnov);
 
 	if (badMagYaw) {
 		_num_bad_flight_yaw_events++;
-	}
 
-	// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
-	if (badMagYaw || !_control_status.flags.yaw_align) {
 		_warning_events.flags.bad_yaw_using_gps_course = true;
 		ECL_WARN("bad yaw, using GPS course");
 
 		// declare the magnetometer as failed if a bad yaw has occurred more than once
-		if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2)
+		if (_control_status.flags.mag_aligned_in_flight
+		    && (_num_bad_flight_yaw_events >= 2)
 		    && !_control_status.flags.mag_fault) {
+
 			_warning_events.flags.stopping_mag_use = true;
 			ECL_WARN("stopping mag use");
 			_control_status.flags.mag_fault = true;
 		}
+	}
+
+	// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
+	if (badMagYaw || !_control_status.flags.yaw_align || force) {
 
 		// calculate new yaw estimate
 		float yaw_new;
@@ -402,8 +409,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 
 		} else {
 			// we don't have wind estimates, so align yaw to the GPS velocity vector
-			yaw_new = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
-
+			yaw_new = gpsCOG;
 		}
 
 		// use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment
@@ -416,7 +422,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 
 		// Use the last magnetometer measurements to reset the field states
 		_state.mag_B.zero();
-		_state.mag_I = _R_to_earth * mag;
+		_state.mag_I = _R_to_earth * _mag_lpf.getState();
 
 		resetMagCov();
 
@@ -435,7 +441,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 		// align mag states only
 
 		// calculate initial earth magnetic field states
-		_state.mag_I = _R_to_earth * mag;
+		_state.mag_I = _R_to_earth * _mag_lpf.getState();
 
 		resetMagCov();
 

src/modules/ekf2/EKF/gps_control.cpp

@@ -91,7 +91,7 @@ void Ekf::controlGpsFusion()
 							// use GPS velocity data to check and correct yaw angle if a FW vehicle
 							if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
 								// if flying a fixed wing aircraft, do a complete reset that includes yaw
-								_mag_yaw_reset_req = true;
+								realignYawGPS();
 							}
 
 							_warning_events.flags.gps_fusion_timout = true;

src/modules/ekf2/EKF/mag_control.cpp

@@ -98,9 +98,6 @@ void Ekf::controlMagFusion()
 		return;
 	}
 
-	_mag_yaw_reset_req |= !_control_status.flags.yaw_align;
-	_mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req;
-
 	if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) {
 
 		const bool mag_enabled_previously = _control_status_prev.flags.mag_hdg || _control_status_prev.flags.mag_3D;
@@ -109,6 +106,7 @@ void Ekf::controlMagFusion()
 		// there are large external disturbances or the more accurate 3-axis fusion
 		switch (_params.mag_fusion_type) {
 		default:
+
 		// FALLTHROUGH
 		case MAG_FUSE_TYPE_AUTO:
 			selectMagAuto();
@@ -128,16 +126,12 @@ void Ekf::controlMagFusion()
 
 		const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
 
-		if (!mag_enabled_previously && mag_enabled) {
-			_mag_yaw_reset_req = true;
-		}
+		if (!_control_status.flags.yaw_align
+		    || _mag_yaw_reset_req || _mag_inhibit_yaw_reset_req
+		    || haglYawResetReq()
+		    || (!mag_enabled_previously && mag_enabled)) {
 
-		if (_control_status.flags.in_air) {
-			checkHaglYawResetReq();
-			runInAirYawReset(mag_sample.mag);
-
-		} else {
-			runOnGroundYawReset();
+			runYawReset();
 		}
 
 		if (!_control_status.flags.yaw_align) {
@@ -152,61 +146,43 @@ void Ekf::controlMagFusion()
 	}
 }
 
-void Ekf::checkHaglYawResetReq()
+bool Ekf::haglYawResetReq() const
 {
 	// We need to reset the yaw angle after climbing away from the ground to enable
 	// recovery from ground level magnetic interference.
-	if (!_control_status.flags.mag_aligned_in_flight) {
+	if (_control_status.flags.in_air && !_control_status.flags.mag_aligned_in_flight) {
 		// Check if height has increased sufficiently to be away from ground magnetic anomalies
 		// and request a yaw reset if not already requested.
 		static constexpr float mag_anomalies_max_hagl = 1.5f;
-		const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
-		_mag_yaw_reset_req = _mag_yaw_reset_req || above_mag_anomalies;
-	}
-}
 
-void Ekf::runOnGroundYawReset()
-{
-	if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) {
-		const bool has_realigned_yaw = canResetMagHeading() ? resetMagHeading() : false;
-
-		if (has_realigned_yaw) {
-			_mag_yaw_reset_req = false;
-			_control_status.flags.yaw_align = true;
-
-			// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
-			// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
-			if (_mag_inhibit_yaw_reset_req) {
-				_mag_inhibit_yaw_reset_req = false;
-				// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
-				P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * _dt_ekf_avg));
-			}
+		if ((getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl) {
+			return true;
 		}
 	}
+
+	return false;
 }
 
-bool Ekf::canResetMagHeading() const
+void Ekf::runYawReset()
 {
-	return !_control_status.flags.mag_field_disturbed && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE);
-}
-
-void Ekf::runInAirYawReset(const Vector3f &mag_sample)
-{
-	if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) {
+	if (!_is_yaw_fusion_inhibited) {
 		bool has_realigned_yaw = false;
 
 		if (_control_status.flags.gps && _control_status.flags.fixed_wing) {
-			has_realigned_yaw = realignYawGPS(mag_sample);
+			has_realigned_yaw = realignYawGPS();
 		}
 
-		if (!has_realigned_yaw && canResetMagHeading()) {
+		if (!has_realigned_yaw) {
 			has_realigned_yaw = resetMagHeading();
 		}
 
 		if (has_realigned_yaw) {
 			_mag_yaw_reset_req = false;
 			_control_status.flags.yaw_align = true;
-			_control_status.flags.mag_aligned_in_flight = true;
+
+			if (_control_status.flags.in_air) {
+				_control_status.flags.mag_aligned_in_flight = true;
+			}
 
 			// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
 			// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
@@ -258,7 +234,7 @@ void Ekf::checkMagBiasObservability()
 	} else if (_mag_bias_observable) {
 		// require sustained yaw motion of 50% the initial yaw rate threshold
 		const float yaw_dt = 1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started);
-		const float min_yaw_change_req =  0.5f * _params.mag_yaw_rate_gate * yaw_dt;
+		const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt;
 		_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
 	}
 
@@ -364,8 +340,7 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 {
 	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
 	// before they are used to constrain heading drift
-	const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6)
-			&& !_control_status.flags.mag_fault && !_control_status.flags.mag_field_disturbed;
+	const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
 
 	if (!_mag_decl_cov_reset) {
 		// After any magnetic field covariance reset event the earth field state