Merge pull request #592 from mavlink/hil_cleanup

HIL cleanup, ensure correct init in X-Plane

src/comm/QGCFlightGearLink.cc

@@ -47,7 +47,7 @@ QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString startupArguments
     flightGearVersion(0),
     startupArguments(startupArguments),
     _sensorHilEnabled(true),
-    barometerOffsetkPa(0.0)
+    barometerOffsetkPa(0.0f)
 {
     this->host = host;
     this->port = port+mav->getUASID();

src/comm/QGCHilLink.h

@@ -72,8 +72,6 @@ public slots:
 protected:
     virtual void setName(QString name) = 0;
 
-    static const unsigned int barometerOffsetkPa = 2;
-
 signals:
     /**
      * @brief This signal is emitted instantly when the link is connected

src/comm/QGCXPlaneLink.cc

@@ -48,10 +48,12 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress
     socket(NULL),
     process(NULL),
     terraSync(NULL),
+    barometerOffsetkPa(15.0f),
     airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN),
     xPlaneConnected(false),
     xPlaneVersion(0),
     simUpdateLast(QGC::groundTimeMilliseconds()),
+    simUpdateFirst(0),
     simUpdateLastText(QGC::groundTimeMilliseconds()),
     simUpdateHz(0),
     _sensorHilEnabled(true)
@@ -452,6 +454,10 @@ void QGCXPlaneLink::readBytes()
     {
         xPlaneConnected = true;
 
+        if (oldConnectionState != xPlaneConnected) {
+            simUpdateFirst = QGC::groundTimeMilliseconds();
+        }
+
         for (unsigned i = 0; i < nsegs; i++)
         {
             // Get index
@@ -642,6 +648,11 @@ void QGCXPlaneLink::readBytes()
         qDebug() << "UNKNOWN PACKET:" << data;
     }
 
+    // Wait for 0.5s before actually using the data, so that all fields are filled
+    if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) {
+        return;
+    }
+
     // Send updated state
     if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 3)
     {

src/comm/QGCXPlaneLink.h

@@ -197,6 +197,7 @@ protected:
     float true_airspeed;
     float groundspeed;
     float xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature;
+    float barometerOffsetkPa;
 
     float man_roll, man_pitch, man_yaw;
     QString airframeName;
@@ -204,6 +205,7 @@ protected:
     bool xPlaneConnected;
     unsigned int xPlaneVersion;
     quint64 simUpdateLast;
+    quint64 simUpdateFirst;
     quint64 simUpdateLastText;
     float simUpdateHz;
     bool _sensorHilEnabled;