fix(ekf2): fuse GNSS yaw when the receiver gives no antenna offset

A NaN sensor_gps.heading_offset means the heading is already of the body frame, and EKF2 only replaces it when EKF2_GPS_YAW_OFF is non-zero. Only fuseGnssYaw() guarded against it, so the observation and innovation were NaN, every sample was rejected, and on the ground fusion restarted with a yaw reset every 17 s.

Assisted-by: Claude:claude-opus-5-5

src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp

@@ -162,10 +162,6 @@ void Ekf::fuseGnssYaw(float antenna_yaw_offset)
 		return;
 	}
 
-	if (!PX4_ISFINITE(antenna_yaw_offset)) {
-		antenna_yaw_offset = 0.f;
-	}
-
 	float heading_pred;
 	float heading_innov_var;
 	VectorState H;

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -189,6 +189,15 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 
 		gnss_sample_new.time_us = time_us;
 
+#if defined(CONFIG_EKF2_GNSS_YAW)
+
+		// Without an antenna offset the heading is already that of the body frame
+		if (!PX4_ISFINITE(gnss_sample_new.yaw_offset)) {
+			gnss_sample_new.yaw_offset = 0.f;
+		}
+
+#endif // CONFIG_EKF2_GNSS_YAW
+
 		_gps_buffer->push(gnss_sample_new);
 		_time_last_gps_buffer_push = _time_latest_us;
 

src/modules/ekf2/test/test_EKF_gnss_yaw.cpp

@@ -186,6 +186,26 @@ TEST_F(EkfGpsHeadingTest, yawMinus30)
 	runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
 }
 
+TEST_F(EkfGpsHeadingTest, fuseWithoutAntennaOffset)
+{
+	// GIVEN: a receiver that reports its heading in the body frame, with no antenna offset
+	_sensor_simulator._gps.setYawOffset(NAN);
+	const float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(10.f));
+	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator.runSeconds(1);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
+
+	// WHEN: running for longer than the fusion timeout
+	_sensor_simulator.runSeconds(10);
+
+	// THEN: the heading keeps being fused, without being reset to again
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_TRUE(_ekf->aid_src_gnss_yaw().fused);
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);
+	checkConvergence(gps_heading, 0.05f);
+}
+
 TEST_F(EkfGpsHeadingTest, fallBackToMag)
 {
 	// GIVEN: an initial GPS yaw, not aligned with the current one