AP_GPS: Adding support for the NMEA GPHDT sentence

The NMEA GPHDT sentence can be used to determine the vehicles bearing
instead of a compass even when the vehicle is stationary.  This type
of GPS is normally very expensive and does the bearing using some sort
of phase ambituity algorithm.

libraries/AP_GPS/AP_GPS.cpp

@@ -72,7 +72,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Param: TYPE
     // @DisplayName: GPS type
     // @Description: GPS type
-    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA
+    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA
     // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("TYPE",    0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT),
@@ -80,7 +80,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Param: TYPE2
     // @DisplayName: 2nd GPS type
     // @Description: GPS type of 2nd GPS
-    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA
+    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA
     // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("TYPE2",   1, AP_GPS, _type[1], 0),
@@ -488,7 +488,11 @@ void AP_GPS::detect_instance(uint8_t instance)
         dstate->last_baud_change_ms = now;
 
         if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) {
-            send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
+            if (_type[instance] == GPS_TYPE_HEMI) {
+                send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING));
+            } else {
+                send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
+            }
         }
     }
 
@@ -540,7 +544,8 @@ void AP_GPS::detect_instance(uint8_t instance)
         else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) &&
                  AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) {
             new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]);
-        } else if (_type[instance] == GPS_TYPE_NMEA &&
+        } else if ((_type[instance] == GPS_TYPE_NMEA ||
+                    _type[instance] == GPS_TYPE_HEMI) &&
                    AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
             new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
         }

libraries/AP_GPS/AP_GPS.h

@@ -84,7 +84,8 @@ public:
         GPS_TYPE_GSOF  = 11,
         GPS_TYPE_ERB = 13,
         GPS_TYPE_MAV = 14,
-        GPS_TYPE_NOVA = 15
+        GPS_TYPE_NOVA = 15,
+        GPS_TYPE_HEMI = 16, // hemisphere NMEA
     };
 
     /// GPS status codes
@@ -130,6 +131,7 @@ public:
         Location location;                  ///< last fix location
         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)
         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
@@ -141,6 +143,7 @@ public:
         bool have_speed_accuracy;         ///< does GPS give speed accuracy? Set to true only once available.
         bool have_horizontal_accuracy;    ///< does GPS give horizontal position accuracy? Set to true only once available.
         bool have_vertical_accuracy;      ///< does GPS give vertical position accuracy? Set to true only once available.
+        bool have_gps_yaw;                ///< does GPS give yaw? Set to true only once available.
         uint32_t last_gps_time_ms;          ///< the system time we got the last GPS timestamp, milliseconds
         uint32_t uart_timestamp_ms;         ///< optional timestamp from set_uart_timestamp()
 
@@ -255,6 +258,23 @@ public:
         return ground_course_cd(primary_instance);
     }
 
+    // yaw in degrees if available
+    bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg) const {
+        if (!have_gps_yaw(instance)) {
+            return false;
+        }
+        yaw_deg = state[instance].gps_yaw;
+        // None of the GPS backends can provide this yet, so we hard
+        // code a fixed value of 10 degrees, which seems like a
+        // reasonable guess. Once a backend can provide a proper
+        // estimate we can implement it
+        accuracy_deg = 10;
+        return true;
+    }
+    bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const {
+        return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg);
+    }
+
     // number of locked satellites
     uint8_t num_sats(uint8_t instance) const {
         return state[instance].num_sats;
@@ -329,6 +349,14 @@ public:
         return have_vertical_velocity(primary_instance);
     }
 
+    // return true if the GPS supports yaw
+    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);
+    }
+    
     // 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
     bool get_lag(uint8_t instance, float &lag_sec) const;

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -223,6 +223,12 @@ bool AP_GPS_NMEA::_have_new_message()
     if (_last_VTG_ms != 0) {
         _last_VTG_ms = 1;
     }
+
+    if (now - _last_HDT_ms > 300) {
+        // we have lost GPS yaw
+        state.have_gps_yaw = false;
+    }
+
     _last_GGA_ms = 1;
     _last_RMC_ms = 1;
     return true;
@@ -293,6 +299,11 @@ bool AP_GPS_NMEA::_term_complete()
                     fill_3d_velocity();
                     // VTG has no fix indicator, can't change fix status
                     break;
+                case _GPS_SENTENCE_HDT:
+                    _last_HDT_ms = now;
+                    state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
+                    state.have_gps_yaw = true;
+                    break;
                 }
             } else {
                 switch (_sentence_type) {
@@ -327,6 +338,10 @@ bool AP_GPS_NMEA::_term_complete()
             _sentence_type = _GPS_SENTENCE_RMC;
         } else if (strcmp(term_type, "GGA") == 0) {
             _sentence_type = _GPS_SENTENCE_GGA;
+        } else if (strcmp(term_type, "HDT") == 0) {
+            _sentence_type = _GPS_SENTENCE_HDT;
+            // HDT doesn't have a data qualifier
+            _gps_data_good = true;
         } else if (strcmp(term_type, "VTG") == 0) {
             _sentence_type = _GPS_SENTENCE_VTG;
             // VTG may not contain a data qualifier, presume the solution is good
@@ -338,7 +353,7 @@ bool AP_GPS_NMEA::_term_complete()
         return false;
     }
 
-    // 32 = RMC, 64 = GGA, 96 = VTG
+    // 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT
     if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
         switch (_sentence_type + _term_number) {
         // operational status
@@ -400,6 +415,9 @@ bool AP_GPS_NMEA::_term_complete()
         case _GPS_SENTENCE_VTG + 5: // Speed (VTG)
             _new_speed = (_parse_decimal_100(_term) * 514) / 1000;       // knots-> m/sec, approximiates * 0.514
             break;
+        case _GPS_SENTENCE_HDT + 1: // Course (HDT)
+            _new_gps_yaw = _parse_decimal_100(_term);
+            break;
         case _GPS_SENTENCE_RMC + 8: // Course (GPRMC)
         case _GPS_SENTENCE_VTG + 1: // Course (VTG)
             _new_course = _parse_decimal_100(_term);

libraries/AP_GPS/AP_GPS_NMEA.h

@@ -71,6 +71,7 @@ private:
         _GPS_SENTENCE_RMC = 32,
         _GPS_SENTENCE_GGA = 64,
         _GPS_SENTENCE_VTG = 96,
+        _GPS_SENTENCE_HDT = 128,
         _GPS_SENTENCE_OTHER = 0
     };
 
@@ -139,6 +140,7 @@ private:
     int32_t _new_altitude;                                      ///< altitude parsed from a term
     int32_t _new_speed;                                                 ///< speed parsed from a term
     int32_t _new_course;                                        ///< course parsed from a term
+    float   _new_gps_yaw;                                        ///< yaw parsed from a term
     uint16_t _new_hdop;                                                 ///< HDOP parsed from a term
     uint8_t _new_satellite_count;                       ///< satellite count parsed from a term
     uint8_t _new_quality_indicator;                                     ///< GPS quality indicator parsed from a term
@@ -146,6 +148,7 @@ private:
     uint32_t _last_RMC_ms = 0;
     uint32_t _last_GGA_ms = 0;
     uint32_t _last_VTG_ms = 0;
+    uint32_t _last_HDT_ms = 0;
 
     /// @name	Init strings
     ///			In ::init, an attempt is made to configure the GPS
@@ -159,3 +162,11 @@ private:
 
     static const char _initialisation_blob[];
 };
+
+#define AP_GPS_NMEA_HEMISPHERE_INIT_STRING \
+        "$JATT,NMEAHE,0\r\n" /* Prefix of GP on the HDT message */      \
+        "$JASC,GPGGA,5\r\n" /* GGA at 5Hz */                            \
+        "$JASC,GPRMC,5\r\n" /* RMC at 5Hz */                            \
+        "$JASC,GPVTG,5\r\n" /* VTG at 5Hz */                            \
+        "$JASC,GPHDT,5\r\n" /* HDT at 5Hz */                            \
+        "$JMODE,SBASR,YES\r\n" /* Enable SBAS */