From 738d000f869c2680c758eeee5f001d8fd9aaf76b Mon Sep 17 00:00:00 2001
From: Philippe Hamelin <hamelin.philippe@ireq.ca>
Date: Tue, 7 Jul 2020 09:16:22 -0400
Subject: [PATCH] AP_GPS: Add NMEA THS message.

---
 libraries/AP_GPS/AP_GPS_NMEA.cpp | 19 ++++++++++++++++---
 libraries/AP_GPS/AP_GPS_NMEA.h   |  3 ++-
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index 8235c68605..2fa579989a 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -236,7 +236,7 @@ bool AP_GPS_NMEA::_have_new_message()
         _last_VTG_ms = 1;
     }
 
-    if (now - _last_HDT_ms > 300) {
+    if (now - _last_HDT_THS_ms > 300) {
         // we have lost GPS yaw
         state.have_gps_yaw = false;
     }
@@ -335,7 +335,8 @@ bool AP_GPS_NMEA::_term_complete()
                     // VTG has no fix indicator, can't change fix status
                     break;
                 case _GPS_SENTENCE_HDT:
-                    _last_HDT_ms = now;
+                case _GPS_SENTENCE_THS:
+                    _last_HDT_THS_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
@@ -368,6 +369,10 @@ bool AP_GPS_NMEA::_term_complete()
                     // Only these sentences give us information about
                     // fix status.
                     state.status = AP_GPS::NO_FIX;
+                    break;
+                case _GPS_SENTENCE_THS:
+                    state.have_gps_yaw = false;
+                    break;
                 }
             }
             // see if we got a good message
@@ -405,6 +410,8 @@ bool AP_GPS_NMEA::_term_complete()
             _sentence_type = _GPS_SENTENCE_HDT;
             // HDT doesn't have a data qualifier
             _gps_data_good = true;
+        } else if (strcmp(term_type, "THS") == 0) {
+            _sentence_type = _GPS_SENTENCE_THS;
         } else if (strcmp(term_type, "VTG") == 0) {
             _sentence_type = _GPS_SENTENCE_VTG;
             // VTG may not contain a data qualifier, presume the solution is good
@@ -416,7 +423,7 @@ bool AP_GPS_NMEA::_term_complete()
         return false;
     }
 
-    // 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT
+    // 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT, 160 = THS
     if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
         switch (_sentence_type + _term_number) {
         // operational status
@@ -428,6 +435,9 @@ bool AP_GPS_NMEA::_term_complete()
             _gps_data_good = _term[0] > '0';
             _new_quality_indicator = _term[0] - '0';
             break;
+        case _GPS_SENTENCE_THS + 2: // validity (THS)
+            _gps_data_good = _term[0] == 'A';
+            break;
         case _GPS_SENTENCE_VTG + 9: // validity (VTG) (we may not see this field)
             _gps_data_good = _term[0] != 'N';
             break;
@@ -481,6 +491,9 @@ bool AP_GPS_NMEA::_term_complete()
         case _GPS_SENTENCE_HDT + 1: // Course (HDT)
             _new_gps_yaw = _parse_decimal_100(_term);
             break;
+        case _GPS_SENTENCE_THS + 1: // Course (THS)
+            _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);
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h
index 0d26ad5987..c2ab117a33 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.h
+++ b/libraries/AP_GPS/AP_GPS_NMEA.h
@@ -73,6 +73,7 @@ private:
         _GPS_SENTENCE_VTG = 96,
         _GPS_SENTENCE_HDT = 128,
         _GPS_SENTENCE_PHD = 138, // extension for AllyStar GPS modules
+        _GPS_SENTENCE_THS = 160, // True heading with quality indicator, available on Trimble MB-Two
         _GPS_SENTENCE_OTHER = 0
     };
 
@@ -143,7 +144,7 @@ private:
     uint32_t _last_RMC_ms;
     uint32_t _last_GGA_ms;
     uint32_t _last_VTG_ms;
-    uint32_t _last_HDT_ms;
+    uint32_t _last_HDT_THS_ms;
     uint32_t _last_PHD_12_ms;
     uint32_t _last_PHD_26_ms;
     uint32_t _last_fix_ms;