ekf2: wrap_pi fixes and fuseYaw() renaming

- to stay consistent with https://github.com/PX4/PX4-Autopilot/pull/19434

src/modules/ekf2/EKF/control.cpp

@@ -366,9 +366,9 @@ void Ekf::controlExternalVisionFusion()
 
 			float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat);
 
-			float innovation = wrap_pi(getEulerYaw(_R_to_earth)) - wrap_pi(measured_hdg);
+			float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
 			float obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f);
-			updateQuaternion(innovation, obs_var);
+			fuseYaw(innovation, obs_var);
 		}
 
 		// record observation and estimate for use next time

src/modules/ekf2/EKF/ekf.cpp

@@ -200,7 +200,7 @@ bool Ekf::initialiseFilter()
 		// rotate the magnetometer measurements into earth frame using a zero yaw angle
 		// the angle of the projection onto the horizontal gives the yaw angle
 		const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState();
-		float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
+		float yaw_new = wrap_pi(-atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination());
 
 		// update quaternion states and corresponding covarainces
 		resetQuatStateYaw(yaw_new, 0.f, false);
@@ -208,6 +208,7 @@ bool Ekf::initialiseFilter()
 		// set the earth magnetic field states using the updated rotation
 		_state.mag_I = _R_to_earth * _mag_lpf.getState();
 		_state.mag_B.zero();
+
 	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
 
 		float yaw_new = wrap_pi(math::radians(_params.heading_init_deg));

src/modules/ekf2/EKF/ekf.h

@@ -590,7 +590,7 @@ private:
 	// update quaternion states and covariances using a yaw innovation and yaw observation variance
 	// innovation : prediction - measurement
 	// variance : observaton variance
-	bool updateQuaternion(const float innovation, const float variance);
+	bool fuseYaw(const float innovation, const float variance);
 
 	// fuse the yaw angle obtained from a dual antenna GPS unit
 	void fuseGpsYaw();

src/modules/ekf2/EKF/mag_control.cpp

@@ -327,14 +327,14 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 
 		// the angle of the projection onto the horizontal gives the yaw angle
 		// calculate the yaw innovation and wrap to the interval between +-pi
-		float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
+		float measured_hdg = wrap_pi(-atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination());
 
-		float innovation = wrap_pi(getEulerYaw(_R_to_earth)) - wrap_pi(measured_hdg);
+		float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
 
 		float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);
 
 		// Update the quaternion states and covariance matrix
-		if (updateQuaternion(innovation, obs_var)) {
+		if (fuseYaw(innovation, obs_var)) {
 			_time_last_mag_heading_fuse = _time_last_imu;
 		}
 	}

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -434,7 +434,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 }
 
 // update quaternion states and covariances using the yaw innovation and yaw observation variance
-bool Ekf::updateQuaternion(const float innovation, const float variance)
+bool Ekf::fuseYaw(const float innovation, const float variance)
 {
 	// assign intermediate state variables
 	const float q0 = _state.quat_nominal(0);

src/modules/ekf2/EKF/zero_innovation_heading_update.cpp

@@ -44,18 +44,18 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 	// bias learning when stationary on ground and to prevent uncontrolled yaw variance growth
 	if (_control_status.flags.vehicle_at_rest && _control_status.flags.tilt_align) {
 
-		const float euler_yaw = wrap_pi(getEulerYaw(_R_to_earth));
+		const float euler_yaw = getEulerYaw(_R_to_earth);
 
 		// record static yaw when transitioning to at rest
 		if (!PX4_ISFINITE(_last_static_yaw)) {
 			_last_static_yaw = euler_yaw;
 		}
 
-		// fuse last static yaw at a limited rate (every 200 milliseconds)
-		if (isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
-			float innovation = euler_yaw - _last_static_yaw;
+		// fuse last static yaw at a limited rate (every 1000 milliseconds)
+		if (isTimedOut(_time_last_heading_fuse, (uint64_t)1'000'000)) {
+			float innovation = wrap_pi(euler_yaw - _last_static_yaw);
 			float obs_var = 0.001f;
-			updateQuaternion(innovation, obs_var);
+			fuseYaw(innovation, obs_var);
 		}
 
 	} else {
@@ -67,15 +67,15 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 			// if necessary fuse zero innovation to prevent unconstrained quaternion variance growth
 			float innovation = 0.f;
 			float obs_var = 0.25f;
-			updateQuaternion(innovation, obs_var);
+			fuseYaw(innovation, obs_var);
 
 		} else if (!_control_status.flags.tilt_align) {
 			// fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
 			float innovation = 0.f;
 			float obs_var = 0.01f;
-			updateQuaternion(innovation, obs_var);
+			fuseYaw(innovation, obs_var);
 		}
 
-		_last_static_yaw = wrap_pi(getEulerYaw(_R_to_earth));
+		_last_static_yaw = getEulerYaw(_R_to_earth);
 	}
 }