SITL: Add NMEA THS message.

libraries/AP_HAL_SITL/sitl_gps.cpp

@@ -421,7 +421,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance)
     pvt.headVeh = 0;
     memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2));
 
-    if (_sitl->gps_hdg_enabled[instance]) {
+    if (_sitl->gps_hdg_enabled[instance] > SITL::SITL::GPS_HEADING_NONE) {
         const Vector3f ant1_pos = _sitl->gps_pos_offset[instance^1].get();
         const Vector3f ant2_pos = _sitl->gps_pos_offset[instance].get();
         Vector3f rel_antenna_pos = ant2_pos - ant1_pos;
@@ -444,7 +444,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance)
     _gps_send_ubx(MSG_SOL,    (uint8_t*)&sol, sizeof(sol), instance);
     _gps_send_ubx(MSG_DOP,    (uint8_t*)&dop, sizeof(dop), instance);
     _gps_send_ubx(MSG_PVT,    (uint8_t*)&pvt, sizeof(pvt), instance);
-    if (_sitl->gps_hdg_enabled[instance]) {
+    if (_sitl->gps_hdg_enabled[instance] > SITL::SITL::GPS_HEADING_NONE) {
         _gps_send_ubx(MSG_RELPOSNED,    (uint8_t*)&relposned, sizeof(relposned), instance);
     }
 
@@ -738,9 +738,12 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance)
                      heading,
                      dstring);
 
-    if (_sitl->gps_hdg_enabled[instance]) {
+    if (_sitl->gps_hdg_enabled[instance] == SITL::SITL::GPS_HEADING_HDT) {
         _gps_nmea_printf(instance, "$GPHDT,%.2f,T", d->yaw);
     }
+    else if (_sitl->gps_hdg_enabled[instance] == SITL::SITL::GPS_HEADING_THS) {
+        _gps_nmea_printf(instance, "$GPTHS,%.2f,%c,T", d->yaw, d->have_lock ? 'A' : 'V');
+    }
 }
 
 void SITL_State::_sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload, uint8_t instance)

libraries/SITL/SITL.cpp

@@ -294,7 +294,7 @@ const AP_Param::GroupInfo SITL::var_gps[] = {
     AP_GROUPINFO("GPS_NOISE",     10, SITL,  gps_noise[0], 0),
     AP_GROUPINFO("GPS_LOCKTIME",  11, SITL,  gps_lock_time[0], 0),
     AP_GROUPINFO("GPS_ALT_OFS",   12, SITL,  gps_alt_offset[0], 0),
-    AP_GROUPINFO("GPS_HDG",       13, SITL,  gps_hdg_enabled[0], 0),
+    AP_GROUPINFO("GPS_HDG",       13, SITL,  gps_hdg_enabled[0], SITL::GPS_HEADING_NONE),
     AP_GROUPINFO("GPS_ACC",       14, SITL,  gps_accuracy[0], 0.3),
     AP_GROUPINFO("GPS_VERR",      15, SITL,  gps_vel_err[0], 0),
 
@@ -310,7 +310,7 @@ const AP_Param::GroupInfo SITL::var_gps[] = {
     AP_GROUPINFO("GPS2_NOISE",    39, SITL,  gps_noise[1], 0),
     AP_GROUPINFO("GPS2_LCKTIME",  40, SITL,  gps_lock_time[1], 0),
     AP_GROUPINFO("GPS2_ALT_OFS",  41, SITL,  gps_alt_offset[1], 0),
-    AP_GROUPINFO("GPS2_HDG",      42, SITL,  gps_hdg_enabled[1], 0),
+    AP_GROUPINFO("GPS2_HDG",      42, SITL,  gps_hdg_enabled[1], SITL::GPS_HEADING_NONE),
     AP_GROUPINFO("GPS2_ACC",      43, SITL,  gps_accuracy[1], 0.3),
     AP_GROUPINFO("GPS2_VERR",     44, SITL,  gps_vel_err[1], 0),
 

libraries/SITL/SITL.h

@@ -138,6 +138,12 @@ public:
         GPS_TYPE_SBP2   = 9,
     };
 
+    enum GPSHeading {
+        GPS_HEADING_NONE = 0,
+        GPS_HEADING_HDT  = 1,
+        GPS_HEADING_THS  = 2,
+    };
+
     struct sitl_fdm state;
 
     // loop update rate in Hz