Concept: Separate GNSS position and heading topics

msg/CMakeLists.txt

@@ -186,6 +186,7 @@ set(msg_files
 	vehicle_control_mode.msg
 	vehicle_global_position.msg
 	vehicle_gps_position.msg
+	vehicle_gnss_heading.msg
 	vehicle_imu.msg
 	vehicle_imu_status.msg
 	vehicle_land_detected.msg

msg/vehicle_gnss_heading.msg

@@ -0,0 +1,5 @@
+uint64 timestamp                  # time since system start(microseconds)
+uint64 timestamp_sample           # time since system start(microseconds)
+
+float32 heading                   # Heading angle of XYZ body frame rel to NED.(radians)
+float32 heading_accuracy          # Accuracy of heading of the relative position vector(radians)

src/modules/ekf2/EKF/common.h

@@ -79,6 +79,12 @@ struct gps_message {
 	float pdop;		///< position dilution of precision
 };
 
+struct gnss_heading_message {
+	uint64_t time_usec{0};	///< timestamp of the measurement (uSec)
+	float heading;		///< heading angle in radians
+	float heading_accuracy;	///< heading accuracy in radians
+};
+
 struct outputSample {
 	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
 	Quatf  quat_nominal;	///< nominal quaternion describing vehicle attitude
@@ -112,6 +118,11 @@ struct gpsSample {
 	float	    vacc;	///< 1-std vertical position error (m)
 	float       sacc;	///< 1-std speed error (m/sec)
 };
+struct gnssHeadingSample {
+	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
+	float heading;		///< heading angle in radians
+	float heading_accuracy;	///< heading accuracy in radians
+};
 
 struct magSample {
 	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
@@ -202,10 +213,11 @@ enum TerrainFusionMask : int32_t {
 #define MAG_FUSE_TYPE_NONE	5	///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
 
 // Maximum sensor intervals in usec
-#define GPS_MAX_INTERVAL  (uint64_t)5e5	///< Maximum allowable time interval between GPS measurements (uSec)
-#define BARO_MAX_INTERVAL (uint64_t)2e5	///< Maximum allowable time interval between pressure altitude measurements (uSec)
-#define RNG_MAX_INTERVAL  (uint64_t)2e5	///< Maximum allowable time interval between range finder  measurements (uSec)
-#define EV_MAX_INTERVAL   (uint64_t)2e5	///< Maximum allowable time interval between external vision system measurements (uSec)
+#define GPS_MAX_INTERVAL           (uint64_t)5e5	///< Maximum allowable time interval between GPS measurements (uSec)
+#define GNSS_HEADING_MAX_INTERVAL  (uint64_t)5e5	///< Maximum allowable time interval between GNSS heading measurements (uSec)
+#define BARO_MAX_INTERVAL          (uint64_t)2e5	///< Maximum allowable time interval between pressure altitude measurements (uSec)
+#define RNG_MAX_INTERVAL           (uint64_t)2e5	///< Maximum allowable time interval between range finder  measurements (uSec)
+#define EV_MAX_INTERVAL            (uint64_t)2e5	///< Maximum allowable time interval between external vision system measurements (uSec)
 
 // bad accelerometer detection and mitigation
 #define BADACC_PROBATION  (uint64_t)10e6	///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)

src/modules/ekf2/EKF/control.cpp

@@ -123,6 +123,12 @@ void Ekf::controlFusionModes()
 		}
 	}
 
+	if (_gnss_heading_buffer) {
+		// check for arrival of new sensor data at the fusion time horizon
+		_time_prev_gnss_heading_us = _gnss_heading_sample_delayed.time_us;
+		_gnss_heading_data_ready = _gnss_heading_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gnss_heading_sample_delayed);
+	}
+
 	if (_range_buffer) {
 		// Get range data from buffer and check validity
 		bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
@@ -179,6 +185,7 @@ void Ekf::controlFusionModes()
 	controlMagFusion();
 	controlOpticalFlowFusion();
 	controlGpsFusion();
+	controlGNSSHeadingFusion();
 	controlAirDataFusion();
 	controlBetaFusion();
 	controlDragFusion();
@@ -554,7 +561,7 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks()
 	_time_good_motion_us = _imu_sample_delayed.time_us;
 }
 
-void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
+void Ekf::controlGNSSHeadingFusion()
 {
 	if (!(_params.fusion_mode & MASK_USE_GPSYAW)
 	    || _control_status.flags.gps_yaw_fault) {
@@ -563,29 +570,20 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 		return;
 	}
 
-	const bool is_new_data_available = PX4_ISFINITE(_gps_sample_delayed.yaw);
-
-	if (is_new_data_available) {
-
-		const bool continuing_conditions_passing = !gps_checks_failing;
+	if (_gnss_heading_data_ready) {
 
-		const bool is_gps_yaw_data_intermittent = !isRecent(_time_last_gps_yaw_data, 2 * GPS_MAX_INTERVAL);
+		const bool is_gnss_heading_data_intermittent = !isRecent(_time_last_gnss_heading_data, 2 * GNSS_HEADING_MAX_INTERVAL);
 
-		const bool starting_conditions_passing = continuing_conditions_passing
-				&& _control_status.flags.tilt_align
-				&& gps_checks_passing
-				&& !is_gps_yaw_data_intermittent
-				&& !_gps_intermittent;
+		const bool starting_conditions_passing = _control_status.flags.tilt_align
+				&& !is_gnss_heading_data_intermittent;
 
-		_time_last_gps_yaw_data = _time_last_imu;
+		_time_last_gnss_heading_data = _time_last_imu;
 
 		if (_control_status.flags.gps_yaw) {
 
-			if (continuing_conditions_passing) {
-
 				fuseGpsYaw();
 
-				const bool is_fusion_failing = isTimedOut(_time_last_gps_yaw_fuse, _params.reset_timeout_max);
+				const bool is_fusion_failing = isTimedOut(_time_last_gnss_heading_fuse, _params.reset_timeout_max);
 
 				if (is_fusion_failing) {
 					if (_nb_gps_yaw_reset_available > 0) {
@@ -611,10 +609,6 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 					// TODO: should we give a new reset credit when the fusion does not fail for some time?
 				}
 
-			} else {
-				// Stop GPS yaw fusion but do not declare it faulty
-				stopGpsYawFusion();
-			}
 
 		} else {
 			if (starting_conditions_passing) {
@@ -627,7 +621,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 			}
 		}
 
-	} else if (_control_status.flags.gps_yaw && isTimedOut(_time_last_gps_yaw_data, _params.reset_timeout_max)) {
+	} else if (_control_status.flags.gps_yaw && isTimedOut(_time_last_gnss_heading_data, _params.reset_timeout_max)) {
 		// No yaw data in the message anymore. Stop until it comes back.
 		stopGpsYawFusion();
 	}

src/modules/ekf2/EKF/ekf.h

@@ -376,13 +376,15 @@ class Ekf final : public EstimatorInterface
 
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
+	bool _gnss_heading_data_ready{false};///< true when new GNSS heading data has fallen behind the fusion time horizon and is available to be fused
 	bool _baro_data_ready{false};	///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
 	bool _ev_data_ready{false};	///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
 	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
 
 	uint64_t _time_prev_gps_us{0};	///< time stamp of previous GPS data retrieved from the buffer (uSec)
+	uint64_t _time_prev_gnss_heading_us{0}; ///< time stamp of previous GNSS heading data retrieved from the buffer (uSec)
 	uint64_t _time_last_aiding{0};	///< amount of time we have been doing inertial only deadreckoning (uSec)
 	bool _using_synthetic_position{false};	///< true if we are using a synthetic position to constrain drift
 
@@ -396,8 +398,8 @@ class Ekf final : public EstimatorInterface
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
 	uint64_t _time_last_fake_pos_fuse{0};	///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
-	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
-	uint64_t _time_last_gps_yaw_data{0};	///< time the last GPS yaw measurement was available (uSec)
+	uint64_t _time_last_gnss_heading_fuse{0};	///< time the last fusion of GNSS heading measurements were performed (uSec)
+	uint64_t _time_last_gnss_heading_data{0};	///< time the last GNSS heading measurement was available (uSec)
 	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
@@ -833,7 +835,7 @@ class Ekf final : public EstimatorInterface
 	bool hasHorizontalAidingTimedOut() const;
 	bool isYawFailure() const;
 
-	void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing);
+	void controlGNSSHeadingFusion();
 
 	// control fusion of magnetometer observations
 	void controlMagFusion();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1559,10 +1559,6 @@ void Ekf::stopGpsFusion()
 		stopGpsVelFusion();
 	}
 
-	if (_control_status.flags.gps_yaw) {
-		stopGpsYawFusion();
-	}
-
 	// We do not need to know the true North anymore
 	// EV yaw can start again
 	_inhibit_ev_yaw_use = false;

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -47,6 +47,7 @@
 EstimatorInterface::~EstimatorInterface()
 {
 	delete _gps_buffer;
+	delete _gnss_heading_buffer;
 	delete _mag_buffer;
 	delete _baro_buffer;
 	delete _range_buffer;
@@ -193,6 +194,41 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 	}
 }
 
+void EstimatorInterface::setGnssHeadingData(const gnss_heading_message &gnss_heading)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_gnss_heading_buffer == nullptr) {
+		_gnss_heading_buffer = new RingBuffer<gnssHeadingSample>(_obs_buffer_length);
+
+		if (_gnss_heading_buffer == nullptr || !_gnss_heading_buffer->valid()) {
+			delete _gnss_heading_buffer;
+			_gnss_heading_buffer = nullptr;
+			printBufferAllocationFailed("GNSS heading");
+			return;
+		}
+	}
+
+	if ((gnss_heading.time_usec - _time_last_gnss_heading) > _min_obs_interval_us) {
+		_time_last_gnss_heading = gnss_heading.time_usec;
+
+		gnssHeadingSample gnss_heading_sample_new;
+
+		gnss_heading_sample_new.time_us = gnss_heading.time_usec - static_cast<uint64_t>(_params.gps_delay_ms * 1000);
+		gnss_heading_sample_new.time_us -= static_cast<uint64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
+
+		gnss_heading_sample_new.heading = gnss_heading.heading;
+		gnss_heading_sample_new.heading_accuracy = gnss_heading.heading_accuracy;
+
+		_gnss_heading_buffer->push(gnss_heading_sample_new);
+	} else {
+		ECL_ERR("GNSS heading data too fast %" PRIu64, gnss_heading.time_usec - _time_last_gnss_heading);
+	}
+}
+
 void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 {
 	if (!_initialised) {
@@ -564,6 +600,10 @@ void EstimatorInterface::print_status()
 		printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
 	}
 
+	if (_gnss_heading_buffer) {
+		printf("gnss heading buffer: %d/%d (%d Bytes)\n", _gnss_heading_buffer->entries(), _gnss_heading_buffer->get_length(), _gnss_heading_buffer->get_total_size());
+	}
+
 	if (_mag_buffer) {
 		printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
 	}

src/modules/ekf2/EKF/estimator_interface.h

@@ -86,6 +86,8 @@ class EstimatorInterface
 
 	void setGpsData(const gps_message &gps);
 
+	void setGnssHeadingData(const gnss_heading_message &gnss_heading_sample);
+
 	void setBaroData(const baroSample &baro_sample);
 
 	void setAirspeedData(const airspeedSample &airspeed_sample);
@@ -294,6 +296,7 @@ class EstimatorInterface
 	// measurement samples capturing measurements on the delayed time horizon
 	baroSample _baro_sample_delayed{};
 	gpsSample _gps_sample_delayed{};
+	gnssHeadingSample _gnss_heading_sample_delayed{};
 	sensor::SensorRangeFinder _range_sensor{};
 	airspeedSample _airspeed_sample_delayed{};
 	flowSample _flow_sample_delayed{};
@@ -363,6 +366,7 @@ class EstimatorInterface
 	RingBuffer<outputVert> _output_vert_buffer{12};
 
 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
+	RingBuffer<gnssHeadingSample> *_gnss_heading_buffer{nullptr};
 	RingBuffer<magSample> *_mag_buffer{nullptr};
 	RingBuffer<baroSample> *_baro_buffer{nullptr};
 	RingBuffer<rangeSample> *_range_buffer{nullptr};
@@ -375,6 +379,7 @@ class EstimatorInterface
 	// timestamps of latest in buffer saved measurement in microseconds
 	uint64_t _time_last_imu{0};
 	uint64_t _time_last_gps{0};
+	uint64_t _time_last_gnss_heading{0};
 	uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec)
 	uint64_t _time_last_baro{0};
 	uint64_t _time_last_range{0};

src/modules/ekf2/EKF/gps_control.cpp

@@ -51,8 +51,6 @@ void Ekf::controlGpsFusion()
 		const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
 		const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
 
-		controlGpsYawFusion(gps_checks_passing, gps_checks_failing);
-
 		// 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
 		const bool mandatory_conditions_passing = _control_status.flags.tilt_align

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -53,16 +53,12 @@ void Ekf::fuseGpsYaw()
 	const float &q2 = _state.quat_nominal(2);
 	const float &q3 = _state.quat_nominal(3);
 
-	// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
-	const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);
-
-	// define the predicted antenna array vector and rotate into earth frame
-	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
-	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
+	// First column of rotation matrix = body x-axis in earth frame
+	const Vector3f ant_vec_ef = _R_to_earth.col(0);
 
 	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
-	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
-		return;
+	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) {
+	return;
 	}
 
 	// calculate predicted antenna yaw angle
@@ -138,7 +134,7 @@ void Ekf::fuseGpsYaw()
 
 	// calculate the innovation and define the innovation gate
 	const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
-	_heading_innov = predicted_hdg - measured_hdg;
+	_heading_innov = predicted_hdg - _gnss_heading_sample_delayed.heading;
 
 	// wrap the innovation to the interval between +-pi
 	_heading_innov = wrap_pi(_heading_innov);
@@ -191,28 +187,26 @@ void Ekf::fuseGpsYaw()
 	_fault_status.flags.bad_hdg = !is_fused;
 
 	if (is_fused) {
-		_time_last_gps_yaw_fuse = _time_last_imu;
+		_time_last_gnss_heading_fuse = _time_last_imu;
 	}
 }
 
 bool Ekf::resetYawToGps()
 {
-	// define the predicted antenna array vector and rotate into earth frame
-	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
-	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
+	// First column of rotation matrix = body x-axis in earth frame
+	const Vector3f ant_vec_ef = _R_to_earth.col(0);
 
 	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
-	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
+	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) {
 		return false;
 	}
 
-	// GPS yaw measurement is alreday compensated for antenna offset in the driver
-	const float measured_yaw = _gps_sample_delayed.yaw;
+	// TODO: Consider using the reported variance
 
 	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
-	resetQuatStateYaw(measured_yaw, yaw_variance, true);
+	resetQuatStateYaw(_gnss_heading_sample_delayed.heading, yaw_variance, true);
 
-	_time_last_gps_yaw_fuse = _time_last_imu;
+	_time_last_gnss_heading_fuse = _time_last_imu;
 	_yaw_signed_test_ratio_lpf.reset(0.f);
 
 	return true;