AP_GPS: provide yaw feedback in GPS_RAW_INT and GPS2_RAW

allow GPS to display status of GPS yaw for moving baseline and NMEA

libraries/AP_GPS/AP_GPS.cpp

@@ -1036,6 +1036,28 @@ void AP_GPS::inject_data(uint8_t instance, const uint8_t *data, uint16_t len)
     }
 }
 
+/*
+  get GPS yaw following mavlink GPS_RAW_INT and GPS2_RAW
+  convention. We return 0 if the GPS is not configured to provide
+  yaw. We return 65535 for a GPS configured to provide yaw that can't
+  currently provide it. We return from 1 to 36000 for yaw otherwise
+ */
+uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const
+{
+    if (!have_gps_yaw_configured(instance)) {
+        return 0;
+    }
+    float yaw_deg, accuracy_deg;
+    if (!gps_yaw_deg(instance, yaw_deg, accuracy_deg)) {
+        return 65535;
+    }
+    int yaw_cd = wrap_360_cd(yaw_deg * 100);
+    if (yaw_cd == 0) {
+        return 36000;
+    }
+    return yaw_cd;
+}
+
 void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
 {
     const Location &loc = location(0);
@@ -1061,7 +1083,8 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
         hacc * 1000,          // one-sigma standard deviation in mm
         vacc * 1000,          // one-sigma standard deviation in mm
         sacc * 1000,          // one-sigma standard deviation in mm/s
-        0);                   // TODO one-sigma heading accuracy standard deviation
+        0,                    // TODO one-sigma heading accuracy standard deviation
+        gps_yaw_cdeg(0));
 }
 
 #if GPS_MAX_RECEIVERS > 1
@@ -1086,7 +1109,8 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
         ground_course(1)*100, // 1/100 degrees,
         num_sats(1),
         state[1].rtk_num_sats,
-        state[1].rtk_age_ms);
+        state[1].rtk_age_ms,
+        gps_yaw_cdeg(1));
 }
 #endif // GPS_MAX_RECEIVERS
 

libraries/AP_GPS/AP_GPS.h

@@ -147,6 +147,7 @@ public:
         float ground_speed;                 ///< ground speed in m/sec
         float ground_course;                ///< ground course in degrees
         float gps_yaw;                      ///< GPS derived yaw information, if available (degrees)
+        bool  gps_yaw_configured;           ///< GPS is configured to provide yaw
         uint16_t hdop;                      ///< horizontal dilution of precision in cm
         uint16_t vdop;                      ///< vertical dilution of precision in cm
         uint8_t num_sats;                   ///< Number of visible satellites
@@ -367,13 +368,20 @@ public:
         return have_vertical_velocity(primary_instance);
     }
 
-    // return true if the GPS supports yaw
+    // return true if the GPS currently has yaw available
     bool have_gps_yaw(uint8_t instance) const {
         return state[instance].have_gps_yaw;
     }
     bool have_gps_yaw(void) const {
         return have_gps_yaw(primary_instance);
     }
+
+    // return true if the GPS is configured to provide yaw. This will
+    // be true if we expect the GPS to provide yaw, even if it
+    // currently is not able to provide yaw
+    bool have_gps_yaw_configured(uint8_t instance) const {
+        return state[instance].gps_yaw_configured;
+    }
     
     // the expected lag (in seconds) in the position and velocity readings from the gps
     // return true if the GPS hardware configuration is known or the lag parameter has been set manually
@@ -590,7 +598,10 @@ private:
 
     /// Update primary instance
     void update_primary(void);
-    
+
+    // helper function for mavlink gps yaw
+    uint16_t gps_yaw_cdeg(uint8_t instance) const;
+
     // Auto configure types
     enum GPS_AUTO_CONFIG {
         GPS_AUTO_CONFIG_DISABLE = 0,

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -295,6 +295,11 @@ bool AP_GPS_NMEA::_term_complete()
                     _last_HDT_ms = now;
                     state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
                     state.have_gps_yaw = true;
+                    // remember that we are setup to provide yaw. With
+                    // a NMEA GPS we can only tell if the GPS is
+                    // configured to provide yaw when it first sends a
+                    // HDT sentence.
+                    state.gps_yaw_configured = true;
                     break;
                 }
             } else {

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -96,11 +96,12 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
         if (rtcm3_parser == nullptr) {
             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "u-blox %d: failed RTCMv3 parser allocation", state.instance + 1);
         }
-    }
-    if (role == AP_GPS::GPS_ROLE_MB_BASE ||
-        role == AP_GPS::GPS_ROLE_MB_ROVER) {
         _unconfigured_messages |= CONFIG_RTK_MOVBASE;
     }
+    if (role == AP_GPS::GPS_ROLE_MB_ROVER) {
+        _unconfigured_messages |= CONFIG_RTK_MOVBASE;
+        state.gps_yaw_configured = true;
+    }
 }
 
 AP_GPS_UBLOX::~AP_GPS_UBLOX()
@@ -1358,17 +1359,9 @@ AP_GPS_UBLOX::_parse_gps(void)
                 state.have_gps_yaw = true;
                 state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
                 state.have_gps_yaw_accuracy = true;
-                if (last_yaw_ms == 0) {
-                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS: got yaw lock");
-                }
-                last_yaw_ms = AP_HAL::millis();
             } else {
                 state.have_gps_yaw = false;
                 state.have_gps_yaw_accuracy = false;
-                if (last_yaw_ms && AP_HAL::millis() - last_yaw_ms > 5000) {
-                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS: lost yaw lock 0x%x", unsigned(_buffer.relposned.flags));
-                    last_yaw_ms = 0;
-                }
             }
         }
         break;

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -769,5 +769,4 @@ private:
 
     // RTCM3 parser for when in moving baseline base mode
     RTCM3_Parser *rtcm3_parser;
-    uint32_t last_yaw_ms;
 };