refactor(ekf2): separate GNSS heading from position into independent topic

msg/CMakeLists.txt

@@ -234,6 +234,7 @@ set(msg_files
 	VehicleAirData.msg
 	VehicleAngularAccelerationSetpoint.msg
 	VehicleConstraints.msg
+	VehicleGnssHeading.msg
 	VehicleImu.msg
 	VehicleImuStatus.msg
 	VehicleLocalPositionSetpoint.msg

msg/VehicleGnssHeading.msg

@@ -0,0 +1,12 @@
+# Validated GNSS heading from dual-antenna / moving baseline receiver pair.
+# Published by the sensors module after selecting and validating heading from sensor_gnss_relative
+# or falling back to sensor_gps heading fields. Consumed by EKF2 independently from position/velocity.
+
+uint64 timestamp              # time since system start (microseconds)
+uint64 timestamp_sample       # time since system start (microseconds) - actual measurement time
+
+uint32 device_id              # unique device ID for the sensor that does not change between power cycles
+
+float32 heading               # NED heading from dual antenna array (rad, [-PI, PI])
+float32 heading_accuracy      # 1-sigma heading accuracy (rad)
+float32 heading_offset        # body-frame offset of antenna array (rad, [-PI, PI])

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

@@ -45,7 +45,7 @@
 
 #include <ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h>
 
-void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
+void Ekf::controlGnssYawFusion(const gnssYawSample &gnss_yaw_sample)
 {
 	if (!(_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
 	    || _control_status.flags.gnss_yaw_fault) {
@@ -54,11 +54,11 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
 		return;
 	}
 
-	const bool is_new_data_available = PX4_ISFINITE(gnss_sample.yaw);
+	const bool is_new_data_available = PX4_ISFINITE(gnss_yaw_sample.yaw);
 
 	if (is_new_data_available) {
 
-		updateGnssYaw(gnss_sample);
+		updateGnssYaw(gnss_yaw_sample);
 
 		const bool continuing_conditions_passing = _control_status.flags.tilt_align;
 
@@ -67,13 +67,12 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& _gnss_checks.passed()
-				&& !is_gnss_yaw_data_intermittent
-				&& !_gps_intermittent;
+				&& !is_gnss_yaw_data_intermittent;
 
 		if (_control_status.flags.gnss_yaw) {
 			if (continuing_conditions_passing) {
 
-				fuseGnssYaw(gnss_sample.yaw_offset);
+				fuseGnssYaw(gnss_yaw_sample.yaw_offset);
 
 				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
 
@@ -102,7 +101,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
 				    || !isNorthEastAidingActive()) {
 
 					// Reset before starting the fusion
-					if (resetYawToGnss(gnss_sample.yaw, gnss_sample.yaw_offset)) {
+					if (resetYawToGnss(gnss_yaw_sample.yaw, gnss_yaw_sample.yaw_offset)) {
 
 						resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw);
 
@@ -113,7 +112,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
 				} else if (!_aid_src_gnss_yaw.innovation_rejected) {
 					// Do not force a reset but wait for the consistency check to pass
 					_control_status.flags.gnss_yaw = true;
-					fuseGnssYaw(gnss_sample.yaw_offset);
+					fuseGnssYaw(gnss_yaw_sample.yaw_offset);
 				}
 
 				if (_control_status.flags.gnss_yaw) {
@@ -122,31 +121,26 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
 			}
 		}
 
-	} else if (_control_status.flags.gnss_yaw
-		   && !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_max)) {
-
-		// No yaw data in the message anymore. Stop until it comes back.
-		stopGnssYawFusion();
 	}
 }
 
-void Ekf::updateGnssYaw(const gnssSample &gnss_sample)
+void Ekf::updateGnssYaw(const gnssYawSample &gnss_yaw_sample)
 {
 	// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
-	const float measured_hdg = wrap_pi(gnss_sample.yaw + gnss_sample.yaw_offset);
+	const float measured_hdg = wrap_pi(gnss_yaw_sample.yaw + gnss_yaw_sample.yaw_offset);
 
-	const float yaw_acc = PX4_ISFINITE(gnss_sample.yaw_acc) ? gnss_sample.yaw_acc : 0.f;
+	const float yaw_acc = PX4_ISFINITE(gnss_yaw_sample.yaw_acc) ? gnss_yaw_sample.yaw_acc : 0.f;
 	const float R_YAW = sq(fmaxf(yaw_acc, _params.gnss_heading_noise));
 
 	float heading_pred;
 	float heading_innov_var;
 
 	VectorState H;
-	sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gnss_sample.yaw_offset, R_YAW, FLT_EPSILON,
+	sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gnss_yaw_sample.yaw_offset, R_YAW, FLT_EPSILON,
 					    &heading_pred, &heading_innov_var, &H);
 
 	updateAidSourceStatus(_aid_src_gnss_yaw,
-			      gnss_sample.time_us,                          // sample timestamp
+			      gnss_yaw_sample.time_us,                      // sample timestamp
 			      measured_hdg,                                // observation
 			      R_YAW,                                       // observation variance
 			      wrap_pi(heading_pred - measured_hdg),        // innovation

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

@@ -96,12 +96,26 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 		}
 	}
 
-	if (_gps_data_ready) {
+	// GNSS yaw fusion - independent of position/velocity quality gates
 #if defined(CONFIG_EKF2_GNSS_YAW)
-		const gnssSample &gnss_sample = _gps_sample_delayed;
-		controlGnssYawFusion(gnss_sample);
+
+	if (_gnss_yaw_buffer) {
+		const bool gnss_yaw_data_ready = _gnss_yaw_buffer->pop_first_older_than(imu_delayed.time_us,
+						 &_gnss_yaw_sample_delayed);
+
+		if (gnss_yaw_data_ready) {
+			controlGnssYawFusion(_gnss_yaw_sample_delayed);
+
+		} else if (_control_status.flags.gnss_yaw
+			   && !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_max)) {
+			// No yaw data arriving -- stop fusion
+			stopGnssYawFusion();
+		}
+	}
+
 #endif // CONFIG_EKF2_GNSS_YAW
 
+	if (_gps_data_ready) {
 		controlGnssYawEstimator(_aid_src_gnss_vel);
 
 		bool do_vel_pos_reset = false;

src/modules/ekf2/EKF/common.h

@@ -200,14 +200,18 @@ struct gnssSample {
 	uint8_t     fix_type{};   ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
 	uint8_t     nsats{};      ///< number of satellites used
 	float       pdop{};       ///< position dilution of precision
-	float       yaw{};        ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
-	float       yaw_acc{};    ///< 1-std yaw error (rad)
-	float       yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
 	bool        spoofed{};    ///< true if GNSS data is spoofed
 	bool        jammed{};     ///< true if GNSS data is jammed
 	Vector3f    pos_body{};   ///< position of GPS antenna in body frame (m)
 };
 
+struct gnssYawSample {
+	uint64_t    time_us{};    ///< timestamp of the measurement (uSec)
+	float       yaw{NAN};     ///< yaw angle from dual antenna GNSS (rad, [-PI, PI]); NAN indicates no measurement
+	float       yaw_acc{NAN}; ///< 1-std yaw error (rad); NAN indicates accuracy not provided
+	float       yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
+};
+
 struct magSample {
 	uint64_t    time_us{};  ///< timestamp of the measurement (uSec)
 	Vector3f    mag{};      ///< NED magnetometer body frame measurements (Gauss)

src/modules/ekf2/EKF/ekf.h

@@ -906,7 +906,7 @@ class Ekf final : public EstimatorInterface
 	bool isGnssHgtResetAllowed();
 
 # if defined(CONFIG_EKF2_GNSS_YAW)
-	void controlGnssYawFusion(const gnssSample &gps_sample);
+	void controlGnssYawFusion(const gnssYawSample &gnss_yaw_sample);
 	void stopGnssYawFusion();
 
 	// fuse the yaw angle obtained from a dual antenna GPS unit
@@ -916,7 +916,7 @@ class Ekf final : public EstimatorInterface
 	// return true if the reset was successful
 	bool resetYawToGnss(float gnss_yaw, float gnss_yaw_offset);
 
-	void updateGnssYaw(const gnssSample &gps_sample);
+	void updateGnssYaw(const gnssYawSample &gnss_yaw_sample);
 
 # endif // CONFIG_EKF2_GNSS_YAW
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -48,6 +48,9 @@ EstimatorInterface::~EstimatorInterface()
 {
 #if defined(CONFIG_EKF2_GNSS)
 	delete _gps_buffer;
+# if defined(CONFIG_EKF2_GNSS_YAW)
+	delete _gnss_yaw_buffer;
+# endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	delete _mag_buffer;
@@ -192,19 +195,47 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 		_gps_buffer->push(gnss_sample_new);
 		_time_last_gps_buffer_push = _time_latest_us;
 
+	} else {
+		ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us,
+			 _min_obs_interval_us);
+	}
+}
+
 #if defined(CONFIG_EKF2_GNSS_YAW)
+void EstimatorInterface::setGnssYawData(const gnssYawSample &gnss_yaw_sample)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	if (_gnss_yaw_buffer == nullptr) {
+		_gnss_yaw_buffer = new TimestampedRingBuffer<gnssYawSample>(_obs_buffer_length);
 
-		if (PX4_ISFINITE(gnss_sample.yaw)) {
-			_time_last_gnss_yaw_buffer_push = _time_latest_us;
+		if (_gnss_yaw_buffer == nullptr || !_gnss_yaw_buffer->valid()) {
+			delete _gnss_yaw_buffer;
+			_gnss_yaw_buffer = nullptr;
+			printBufferAllocationFailed("GNSS yaw");
+			return;
 		}
+	}
 
-#endif // CONFIG_EKF2_GNSS_YAW
+	const int64_t time_us = gnss_yaw_sample.time_us - static_cast<int64_t>(_dt_ekf_avg * 5e5f);
+
+	if (time_us >= static_cast<int64_t>(_gnss_yaw_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		gnssYawSample sample_new(gnss_yaw_sample);
+		sample_new.time_us = time_us;
+
+		_gnss_yaw_buffer->push(sample_new);
+		_time_last_gnss_yaw_buffer_push = _time_latest_us;
 
 	} else {
-		ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us,
-			 _min_obs_interval_us);
+		ECL_WARN("GNSS yaw data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us,
+			 _gnss_yaw_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_GNSS_YAW
+
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_BAROMETER)

src/modules/ekf2/EKF/estimator_interface.h

@@ -93,6 +93,10 @@ class EstimatorInterface
 
 	const gnssSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
 
+# if defined(CONFIG_EKF2_GNSS_YAW)
+	void setGnssYawData(const gnssYawSample &gnss_yaw_sample);
+# endif // CONFIG_EKF2_GNSS_YAW
+
 	float gps_horizontal_position_drift_rate_m_s() const { return _gnss_checks.horizontal_position_drift_rate_m_s(); }
 	float gps_vertical_position_drift_rate_m_s() const { return _gnss_checks.vertical_position_drift_rate_m_s(); }
 	float gps_filtered_horizontal_velocity_m_s() const { return _gnss_checks.filtered_horizontal_velocity_m_s(); }
@@ -420,7 +424,8 @@ class EstimatorInterface
 			   _control_status};
 
 # if defined(CONFIG_EKF2_GNSS_YAW)
-	// innovation consistency check monitoring ratios
+	TimestampedRingBuffer<gnssYawSample> *_gnss_yaw_buffer {nullptr};
+	gnssYawSample _gnss_yaw_sample_delayed{};
 	uint64_t _time_last_gnss_yaw_buffer_push{0};
 # endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS

src/modules/ekf2/EKF2.cpp

@@ -807,6 +807,9 @@ void EKF2::Run()
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 #if defined(CONFIG_EKF2_GNSS)
 		UpdateGpsSample(ekf2_timestamps);
+# if defined(CONFIG_EKF2_GNSS_YAW)
+		UpdateGnssYawSample(ekf2_timestamps);
+# endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 		UpdateMagSample(ekf2_timestamps);
@@ -2623,15 +2626,6 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			return; //TODO: change and set to NAN
 		}
 
-		if (fabsf(_param_ekf2_gps_yaw_off.get()) > 0.f) {
-			if (!PX4_ISFINITE(vehicle_gps_position.heading_offset) && PX4_ISFINITE(vehicle_gps_position.heading)) {
-				// Apply offset
-				float yaw_offset = matrix::wrap_pi(math::radians(_param_ekf2_gps_yaw_off.get()));
-				vehicle_gps_position.heading_offset = yaw_offset;
-				vehicle_gps_position.heading = matrix::wrap_pi(vehicle_gps_position.heading - yaw_offset);
-			}
-		}
-
 		const float altitude_amsl = static_cast<float>(vehicle_gps_position.altitude_msl_m);
 		const float altitude_ellipsoid = static_cast<float>(vehicle_gps_position.altitude_ellipsoid_m);
 
@@ -2652,9 +2646,6 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			.nsats = vehicle_gps_position.satellites_used,
 			.pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop
 				      + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
-			.yaw = vehicle_gps_position.heading, //TODO: move to different message
-			.yaw_acc = vehicle_gps_position.heading_accuracy,
-			.yaw_offset = vehicle_gps_position.heading_offset,
 			.spoofed = vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_DETECTED,
 			.jammed = vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_DETECTED,
 			.pos_body = Vector3f(vehicle_gps_position.antenna_offset_x,
@@ -2680,6 +2671,40 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 	}
 }
 
+#if defined(CONFIG_EKF2_GNSS_YAW)
+void EKF2::UpdateGnssYawSample(ekf2_timestamps_s &ekf2_timestamps)
+{
+	vehicle_gnss_heading_s gnss_heading;
+
+	if (_vehicle_gnss_heading_sub.update(&gnss_heading)) {
+
+		float yaw = gnss_heading.heading;
+		float yaw_offset = gnss_heading.heading_offset;
+
+		// Apply EKF2_GPS_YAW_OFF if the driver didn't set an offset
+		if (fabsf(_param_ekf2_gps_yaw_off.get()) > 0.f) {
+			if (!PX4_ISFINITE(yaw_offset) && PX4_ISFINITE(yaw)) {
+				yaw_offset = matrix::wrap_pi(math::radians(_param_ekf2_gps_yaw_off.get()));
+				yaw = matrix::wrap_pi(yaw - yaw_offset);
+			}
+		}
+
+		if (!PX4_ISFINITE(yaw_offset)) {
+			yaw_offset = 0.f;
+		}
+
+		gnssYawSample gnss_yaw_sample{
+			.time_us = (gnss_heading.timestamp_sample > 0) ? gnss_heading.timestamp_sample : gnss_heading.timestamp,
+			.yaw = yaw,
+			.yaw_acc = gnss_heading.heading_accuracy,
+			.yaw_offset = yaw_offset,
+		};
+
+		_ekf.setGnssYawData(gnss_yaw_sample);
+	}
+}
+#endif // CONFIG_EKF2_GNSS_YAW
+
 float EKF2::altEllipsoidToAmsl(float ellipsoid_alt) const
 {
 	return ellipsoid_alt - _geoid_height_lpf.getState();

src/modules/ekf2/EKF2.hpp

@@ -87,6 +87,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_gnss_heading.h>
 #include <uORB/topics/yaw_estimator_status.h>
 
 #if defined(CONFIG_EKF2_AIRSPEED)
@@ -224,6 +225,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 	void PublishGnssHgtBias(const hrt_abstime &timestamp);
 	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
 	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
+# if defined(CONFIG_EKF2_GNSS_YAW)
+	void UpdateGnssYawSample(ekf2_timestamps_s &ekf2_timestamps);
+# endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
@@ -507,6 +511,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 # if defined(CONFIG_EKF2_GNSS_YAW)
 	hrt_abstime _status_gnss_yaw_pub_last {0};
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};
+	uORB::Subscription _vehicle_gnss_heading_sub{ORB_ID(vehicle_gnss_heading)};
 # endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS