Fixed HIL startup crash, fixed yaw offset

src/comm/QGCXPlaneLink.cc

@@ -317,7 +317,7 @@ void QGCXPlaneLink::readBytes()
                 //qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
                 pitch = p.f[0] / 180.0f * M_PI;
                 roll = p.f[1] / 180.0f * M_PI;
-                yaw = (p.f[2] - 180.0f) / 180.0f * M_PI;
+                yaw = p.f[2] / 180.0f * M_PI;
             }
 
 //            else if (p.index == 19)
@@ -552,6 +552,10 @@ void QGCXPlaneLink::setRandomAttitude()
  **/
 bool QGCXPlaneLink::connectSimulation()
 {
+    qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
+
+    start(LowPriority);
+
     if (!mav) return false;
     if (connectState) return false;
 
@@ -772,10 +776,6 @@ bool QGCXPlaneLink::connectSimulation()
 //    qDebug() << "STARTING SIM";
 
 //        qDebug() << "STARTING: " << processFgfs << processCall;
-
-    qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
-
-    start(LowPriority);
     return connectState;
 }
 

src/uas/UAS.cc

@@ -106,7 +106,8 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     connectionLost(false),
     lastVoltageWarning(0),
     lastNonNullTime(0),
-    onboardTimeOffsetInvalidCount(0)
+    onboardTimeOffsetInvalidCount(0),
+    hilEnabled(false)
 
 {
     for (unsigned int i = 0; i<255;++i)
@@ -2605,6 +2606,8 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo
 **/
 void UAS::startHil()
 {
+    if (hilEnabled) return;
+    hilEnabled = true;
     // Connect HIL simulation link
     simulation->connectSimulation();
     mavlink_message_t msg;
@@ -2621,6 +2624,7 @@ void UAS::stopHil()
     mavlink_message_t msg;
     mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & !MAV_MODE_FLAG_HIL_ENABLED, navMode);
     sendMessage(msg);
+    hilEnabled = false;
 }
 
 void UAS::shutdown()

src/uas/UAS.h

@@ -680,6 +680,7 @@ protected:
     quint64 lastVoltageWarning; ///< Time at which the last voltage warning occured
     quint64 lastNonNullTime;    ///< The last timestamp from the MAV that was not null
     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)
 
 protected slots:
     /** @brief Write settings to disk */