diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index 306ce52689d0baf5d1dd80e7809d0c3deaf2c077..04792379f5e915f7eb466bbcd5e4dbd5950b0606 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -185,7 +185,6 @@ void MAVLinkProtocol::linkStatusChanged(bool connected)
             // Start NSH
             const char init[] = {0x0d, 0x0d, 0x0d};
             link->writeBytes(init, sizeof(init));
-            QGC::SLEEP::msleep(500);
 
             // Stop any running mavlink instance
             char* cmd = "mavlink stop\n";
diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc
index c04de3fa1f13a013776aa0c055ed8becbf495a7a..cb5bf335fd86180458461c2918fa3532e5af0841 100644
--- a/src/comm/QGCXPlaneLink.cc
+++ b/src/comm/QGCXPlaneLink.cc
@@ -640,7 +640,7 @@ void QGCXPlaneLink::readBytes()
         float eph = 0.3;
         float epv = 0.6;
         float vel = sqrt(vx*vx + vy*vy + vz*vz);
-        float cog = ((yaw + M_PI) / M_PI) * 180.0f;
+        float cog = atan2(vy, vx);
         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 666109a8235f7e8f989c161387a9466ab971ca72..69a8805d59124e10f083fbd7599686431d142a0d 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -2796,9 +2796,16 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
 
     if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
     {
+        float course = cog;
+        // map to 0..2pi
+        if (course < 0)
+            course += 2.0f * M_PI;
+        // scale from radians to degrees
+        course = (course / M_PI) * 180.0f;
+
         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);
+                                   time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, course*1e2, satellites);
         lastSendTimeGPS = QGC::groundTimeMilliseconds();
         sendMessage(msg);
     }
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index d77f2b5dd1be449e8f843b69c3d1b00addc53803..c13e855600e351ceebabe2111e7e45525e489562 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -549,7 +549,18 @@ public slots:
     void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
                                         float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint16 fields_changed);
 
-    /** @brief Send raw GPS for sensor HIL */
+    /**
+     * @param time_us
+     * @param lat
+     * @param lon
+     * @param alt
+     * @param fix_type
+     * @param eph
+     * @param epv
+     * @param vel
+     * @param cog course over ground, in radians, -pi..pi
+     * @param satellites
+     */
     void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites);