From fdcdbddecc5e9d21453402ee0c2449b2fa385877 Mon Sep 17 00:00:00 2001
From: Grant Morphett <grant@gmorph.com>
Date: Sun, 28 May 2017 14:09:58 +1000
Subject: [PATCH] AP_HAL_SITL: Send NMEA GPHDT sentence if enabled.

and add VTG message
---
 libraries/AP_HAL_SITL/SITL_State.cpp |  5 +++--
 libraries/AP_HAL_SITL/SITL_State.h   |  4 +++-
 libraries/AP_HAL_SITL/sitl_gps.cpp   | 19 ++++++++++++++++++-
 3 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index 3ec68b8ad6..5f7d7171a7 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -85,7 +85,7 @@ void SITL_State::_sitl_setup(const char *home_str)
         // setup some initial values
 #ifndef HIL_MODE
         _update_airspeed(0);
-        _update_gps(0, 0, 0, 0, 0, 0, false);
+        _update_gps(0, 0, 0, 0, 0, 0, 0, false);
         _update_rangefinder(0);
 #endif
         if (enable_gimbal) {
@@ -170,7 +170,7 @@ void SITL_State::_fdm_input_step(void)
     _scheduler->sitl_begin_atomic();
 
     if (_update_count == 0 && _sitl != nullptr) {
-        _update_gps(0, 0, 0, 0, 0, 0, false);
+        _update_gps(0, 0, 0, 0, 0, 0, 0, false);
         _scheduler->timer_event();
         _scheduler->sitl_end_atomic();
         return;
@@ -180,6 +180,7 @@ void SITL_State::_fdm_input_step(void)
         _update_gps(_sitl->state.latitude, _sitl->state.longitude,
                     _sitl->state.altitude,
                     _sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
+                    _sitl->state.yawDeg,
                     !_sitl->gps_disable);
         _update_airspeed(_sitl->state.airspeed);
         _update_rangefinder(_sitl->state.range);
diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h
index f10a144980..bd76094a01 100644
--- a/libraries/AP_HAL_SITL/SITL_State.h
+++ b/libraries/AP_HAL_SITL/SITL_State.h
@@ -102,6 +102,7 @@ private:
         double speedN;
         double speedE;
         double speedD;
+        double yaw;
         bool have_lock;
     };
 
@@ -129,7 +130,8 @@ private:
     uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);
 
     void _update_gps(double latitude, double longitude, float altitude,
-                     double speedN, double speedE, double speedD, bool have_lock);
+                     double speedN, double speedE, double speedD,
+                     double yaw, bool have_lock);
     void _update_airspeed(float airspeed);
     void _update_gps_instance(SITL::SITL::GPSType gps_type, const struct gps_data *d, uint8_t instance);
     void _check_rc_input(void);
diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp
index 1d6f7bf5a3..8fdb5eb97e 100644
--- a/libraries/AP_HAL_SITL/sitl_gps.cpp
+++ b/libraries/AP_HAL_SITL/sitl_gps.cpp
@@ -675,10 +675,20 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance)
                      2.0,
                      d->altitude);
     float speed_knots = norm(d->speedN, d->speedE) * M_PER_SEC_TO_KNOTS;
+
     float heading = ToDeg(atan2f(d->speedE, d->speedN));
     if (heading < 0) {
         heading += 360.0f;
     }
+
+    //$GPVTG,133.18,T,120.79,M,0.11,N,0.20,K,A*24    
+    _gps_nmea_printf(instance, "$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A",
+                     tstring,
+                     heading,
+                     heading,
+                     speed_knots,
+                     speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6);
+    
     _gps_nmea_printf(instance, "$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
                      tstring,
                      d->have_lock?'A':'V',
@@ -687,6 +697,10 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance)
                      speed_knots,
                      heading,
                      dstring);
+
+    if (_sitl->gps_hdg_enabled) {
+        _gps_nmea_printf(instance, "$GPHDT,%.2f,T", d->yaw);
+    }
 }
 
 void SITL_State::_sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload, uint8_t instance)
@@ -1151,7 +1165,8 @@ void SITL_State::_update_gps_file(uint8_t instance)
   possibly send a new GPS packet
  */
 void SITL_State::_update_gps(double latitude, double longitude, float altitude,
-                             double speedN, double speedE, double speedD, bool have_lock)
+                             double speedN, double speedE, double speedD,
+                             double yaw, bool have_lock)
 {
     struct gps_data d;
     char c;
@@ -1195,6 +1210,8 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
 
     d.latitude = latitude;
     d.longitude = longitude;
+    d.yaw = yaw;
+    
     // add an altitude error controlled by a slow sine wave
     d.altitude = altitude + _sitl->gps_noise * sinf(AP_HAL::millis() * 0.0005f);