From f8eddb6d91fdcd1817d1239df20d5c1679accec7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 22 Apr 2013 14:43:32 +0200
Subject: [PATCH] Fixed a bunch of smaller HIL issues, GPS COG is now correct

---
 src/comm/QGCXPlaneLink.cc | 2 +-
 src/uas/UAS.cc            | 8 +++++++-
 src/uas/UAS.h             | 1 +
 3 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc
index 83f834706..a229b59f2 100644
--- a/src/comm/QGCXPlaneLink.cc
+++ b/src/comm/QGCXPlaneLink.cc
@@ -641,7 +641,7 @@ void QGCXPlaneLink::readBytes()
         float eph = 0.3;
         float epv = 0.6;
         float vel = sqrt(vx*vx + vy*vy + vz*vz);
-        float cog = yaw;
+        float cog = ((yaw + M_PI) / M_PI) * 180.0f;
         int satellites = 8;
 
         emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, cog, satellites);
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 64623f4cf..cf0575dc0 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -111,7 +111,8 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     lastVoltageWarning(0),
     lastNonNullTime(0),
     onboardTimeOffsetInvalidCount(0),
-    hilEnabled(false)
+    hilEnabled(false),
+    lastSendTimeGPS(0)
 
 {
     for (unsigned int i = 0; i<255;++i)
@@ -2785,11 +2786,16 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
 
 void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites)
 {
+    // Only send at 10 Hz max rate
+    if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
+        return;
+
     if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
     {
         mavlink_message_t msg;
         mavlink_msg_gps_raw_int_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
                                    time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, cog*1e2, satellites);
+        lastSendTimeGPS = QGC::groundTimeMilliseconds();
         sendMessage(msg);
     }
     else
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index e4dfd783e..4bc794892 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -704,6 +704,7 @@ protected:
     unsigned int onboardTimeOffsetInvalidCount;     ///< Count when the offboard time offset estimation seemed wrong
     bool hilEnabled;            ///< Set to true if HIL mode is enabled from GCS (UAS might be in HIL even if this flag is not set, this defines the GCS HIL setting)
     bool sensorHil;             ///< True if sensor HIL is enabled
+    quint64 lastSendTimeGPS;     ///< Last HIL GPS message sent
 
 protected slots:
     /** @brief Write settings to disk */
-- 
2.22.0