Bugfixed HIL

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";

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);

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);
     }

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);