uavcan: support GNSS heading from relposheading with offset configurable in estimator

* uavcan: add GNSS heading from relposheading
* ekf2: new EKF2_GPS_YAW_OFF parameter to configure any offset in GNSS heading

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>

src/drivers/uavcan/sensors/gnss.cpp

@@ -46,6 +46,7 @@
 #include <drivers/drv_hrt.h>
 #include <systemlib/err.h>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 #include <lib/parameters/param.h>
 
 using namespace time_literals;
@@ -58,6 +59,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
 	_sub_auxiliary(node),
 	_sub_fix(node),
 	_sub_fix2(node),
+	_sub_gnss_heading(node),
 	_pub_moving_baseline_data(node),
 	_pub_rtcm_stream(node),
 	_channel_using_fix2(new bool[_max_channels])
@@ -100,6 +102,12 @@ UavcanGnssBridge::init()
 		return res;
 	}
 
+	res = _sub_gnss_heading.start(RelPosHeadingCbBinder(this, &UavcanGnssBridge::gnss_relative_sub_cb));
+
+	if (res < 0) {
+		PX4_WARN("GNSS relative sub failed %i", res);
+		return res;
+	}
 
 	// UAVCAN_PUB_RTCM
 	int32_t uavcan_pub_rtcm = 0;
@@ -295,6 +303,7 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 		}
 	}
 
+	// Invalidate the heading fields
 	float heading = NAN;
 	float heading_offset = NAN;
 	float heading_accuracy = NAN;
@@ -304,8 +313,9 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 	uint8_t jamming_state = 0;
 	uint8_t spoofing_state = 0;
 
-	// Use ecef_position_velocity for now... There is no heading field
+	// TODO: this hack should eventually be removed now that we have the RelPosHeading message
-	if (!msg.ecef_position_velocity.empty()) {
+	// HACK: Use ecef_position_velocity for heading
+	if (!msg.ecef_position_velocity.empty() && !_rel_heading_valid) {
 		if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[0])) {
 			heading = msg.ecef_position_velocity[0].velocity_xyz[0];
 		}
@@ -328,7 +338,14 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances, heading, heading_offset,
 		     heading_accuracy, noise_per_ms, jamming_indicator, jamming_state, spoofing_state);
 }
+void UavcanGnssBridge::gnss_relative_sub_cb(const
+		uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &msg)
+{
+	_rel_heading_valid = msg.reported_heading_acc_available;
+	_rel_heading = math::radians(msg.reported_heading_deg);
+	_rel_heading_accuracy = math::radians(msg.reported_heading_acc_deg);
 
+}
 template <typename FixType>
 void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
 				    uint8_t fix_type,
@@ -463,9 +480,21 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 		report.vdop = msg.pdop;
 	}
 
-	report.heading = heading;
+	// Use heading from RelPosHeading message if available and we have RTK Fixed solution.
-	report.heading_offset = heading_offset;
+	if (_rel_heading_valid && (fix_type == sensor_gps_s::FIX_TYPE_RTK_FIXED)) {
-	report.heading_accuracy = heading_accuracy;
+		report.heading = _rel_heading;
+		report.heading_offset = NAN;
+		report.heading_accuracy = _rel_heading_accuracy;
+
+		_rel_heading = NAN;
+		_rel_heading_accuracy = NAN;
+		_rel_heading_valid = false;
+
+	} else {
+		report.heading = heading;
+		report.heading_offset = heading_offset;
+		report.heading_accuracy = heading_accuracy;
+	}
 
 	report.noise_per_ms = noise_per_ms;
 	report.jamming_indicator = jamming_indicator;

src/drivers/uavcan/sensors/gnss.hpp

@@ -55,6 +55,7 @@
 #include <uavcan/equipment/gnss/Fix.hpp>
 #include <uavcan/equipment/gnss/Fix2.hpp>
 #include <ardupilot/gnss/MovingBaselineData.hpp>
+#include <ardupilot/gnss/RelPosHeading.hpp>
 #include <uavcan/equipment/gnss/RTCMStream.hpp>
 
 #include <lib/perf/perf_counter.h>
@@ -82,6 +83,7 @@ private:
 	void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &msg);
 	void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
 	void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg);
+	void gnss_relative_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &msg);
 
 	template <typename FixType>
 	void process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
@@ -113,11 +115,16 @@ private:
 		void (UavcanGnssBridge::*)(const uavcan::TimerEvent &)>
 		TimerCbBinder;
 
+	typedef uavcan::MethodBinder < UavcanGnssBridge *,
+		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &) >
+		RelPosHeadingCbBinder;
+
 	uavcan::INode &_node;
 
 	uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxiliaryCbBinder> _sub_auxiliary;
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
+	uavcan::Subscriber<ardupilot::gnss::RelPosHeading, RelPosHeadingCbBinder> _sub_gnss_heading;
 
 	uavcan::Publisher<ardupilot::gnss::MovingBaselineData> _pub_moving_baseline_data;
 	uavcan::Publisher<uavcan::equipment::gnss::RTCMStream> _pub_rtcm_stream;
@@ -137,6 +144,10 @@ private:
 	bool _publish_rtcm_stream{false};
 	bool _publish_moving_baseline_data{false};
 
+	float _rel_heading_accuracy{NAN};
+	float _rel_heading{NAN};
+	bool _rel_heading_valid{false};
+
 	perf_counter_t _rtcm_stream_pub_perf{nullptr};
 	perf_counter_t _moving_baseline_data_pub_perf{nullptr};
 };

src/modules/ekf2/EKF2.cpp

@@ -2405,6 +2405,15 @@ 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);
 

src/modules/ekf2/EKF2.hpp

@@ -537,6 +537,7 @@ private:
 
 		// Used by EKF-GSF experimental yaw estimator
 		(ParamExtFloat<px4::params::EKF2_GSF_TAS>) _param_ekf2_gsf_tas_default,
+		(ParamFloat<px4::params::EKF2_GPS_YAW_OFF>) _param_ekf2_gps_yaw_off,
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_BAROMETER)

src/modules/ekf2/params_gnss.yaml

@@ -46,6 +46,15 @@ parameters:
       min: 1.0
       unit: SD
       decimal: 1
+    EKF2_GPS_YAW_OFF:
+      description:
+        short: Heading/Yaw offset for dual antenna GPS
+      type: float
+      default: 0.0
+      min: 0.0
+      max: 360.0
+      unit: deg
+      decimal: 1
     EKF2_GPS_V_GATE:
       description:
         short: Gate size for GNSS velocity fusion