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/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