AP_GPS: support EAHRS heading as a GPS heading source

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>

Author: Ryanf55

libraries/AP_GPS/AP_GPS.cpp

@@ -231,7 +231,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Param: _DRV_OPTIONS
     // @DisplayName: driver options
     // @Description: Additional backend specific options
-    // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200 on ublox,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health, 6:Enable RTCM full parse even for a single channel, 7:Disable automatic full RTCM parsing when RTCM seen on more than one channel, 8:Force UBlox Config Get/Set for configuration then automatic configuration for Serial GPSes only
+    // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200 on ublox,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health, 6:Enable RTCM full parse even for a single channel, 7:Disable automatic full RTCM parsing when RTCM seen on more than one channel, 8:Force UBlox Config Get/Set for configuration then automatic configuration for Serial GPSes only, 9:Fuse heading from EAHRS GPS when available
     // @User: Advanced
     AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
 

libraries/AP_GPS/AP_GPS.h

@@ -630,7 +630,8 @@ class AP_GPS
         GPSL5HealthOverride = (1U << 5),
         AlwaysRTCMDecode = (1U << 6),
         DisableRTCMDecode = (1U << 7),
-        ForceUBXConfigV2 = (1U << 8U)
+        ForceUBXConfigV2 = (1U << 8U),
+        FuseEARHSHeading = (1U << 9U)
     };
 
     // check if an option is set

libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp

@@ -82,6 +82,22 @@ void AP_GPS_ExternalAHRS::handle_external(const AP_ExternalAHRS::gps_data_messag
 
     state.last_gps_time_ms = AP_HAL::millis();
 
+    if (option_set(AP_GPS::DriverOptions::FuseEARHSHeading) && pkt.have_heading) {
+
+        state.gps_yaw_configured = true;
+        state.gps_yaw = pkt.heading;
+        state.gps_yaw_time_ms = state.last_gps_time_ms;
+        if (pkt.have_heading_acc) {
+            state.have_gps_yaw_accuracy = true;
+            state.gps_yaw_accuracy = pkt.heading_accuracy;
+        } else {
+            state.have_gps_yaw_accuracy = false;
+        }
+    } else {
+        state.gps_yaw_configured = false;
+        state.have_gps_yaw_accuracy = false;
+    }
+
     new_data = true;
 }