Tested FlightGear interface

src/comm/QGCFlightGearLink.cc

@@ -37,15 +37,16 @@ This file is part of the QGROUNDCONTROL project
 #include "QGC.h"
 #include <QHostInfo>
 
-QGCFlightGearLink::QGCFlightGearLink(QHostAddress host, quint16 port)
+QGCFlightGearLink::QGCFlightGearLink(QString remoteHost, QHostAddress host, quint16 port)
 {
     this->host = host;
     this->port = port;
     this->connectState = false;
+    this->currentPort = 49000;
 
     // Set unique ID and add link to the list of links
     this->name = tr("FlightGear Link (port:%1)").arg(port);
-    setRemoteHost(QString("127.0.0.1:%1").arg(port));
+    setRemoteHost(remoteHost);
     connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate()));
     refreshTimer.start(20); // 50 Hz UAV -> Simulation update rate
 }
@@ -122,7 +123,17 @@ void QGCFlightGearLink::updateGlobalPosition(quint64 time, double lat, double lo
 
 void QGCFlightGearLink::sendUAVUpdate()
 {
-    QString state("");
+    // 37.613548,-122.357246,-9999.000000,0.000000,0.424000,297.899994,0.000000\n
+    // magnetos,aileron,elevator,rudder,throttle\n
+
+    float magnetos = 3.0f;
+    float aileron = 0.0f;
+    float elevator = 0.0f;
+    float rudder = 0.0f;
+    float throttle = 90.0f;
+
+    QString state("%1,%2,%3,%4,%5\n");
+    state = state.arg(magnetos).arg(aileron).arg(elevator).arg(rudder).arg(throttle);
     writeBytes(state.toAscii().constData(), state.length());
 }
 

src/comm/QGCFlightGearLink.h

@@ -46,7 +46,7 @@ class QGCFlightGearLink : public QThread
     //Q_INTERFACES(QGCFlightGearLinkInterface:LinkInterface)
 
 public:
-    QGCFlightGearLink(QHostAddress host = QHostAddress::Any, quint16 port = 49005);
+    QGCFlightGearLink(QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005);
     ~QGCFlightGearLink();
 
     bool isConnected();

src/uas/UAS.cc

@@ -74,7 +74,9 @@ statusTimeout(new QTimer(this)),
 paramsOnceRequested(false),
 airframe(0),
 attitudeKnown(false),
-paramManager(NULL)
+paramManager(NULL),
+attitudeStamped(true),
+lastAttitude(0)
 {
     color = UASInterface::getNextColor();
     setBattery(LIPOLY, 3);
@@ -187,7 +189,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
     //    qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq;
 
-    if (message.sysid == uasId)
+    // Only accept messages from this system (condition 1)
+    // and only then if a) attitudeStamped is disabled OR b) attitudeStamped is enabled
+    // and we already got one attitude packet
+    if (message.sysid == uasId && (!attitudeStamped || (attitudeStamped && (lastAttitude != 0)) || message.msgid == MAVLINK_MSG_ID_ATTITUDE))
     {
         QString uasState;
         QString stateDescription;
@@ -457,7 +462,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_attitude_t attitude;
                 mavlink_msg_attitude_decode(&message, &attitude);
-                quint64 time = getUnixTime(attitude.usec);
+                quint64 time = getUnixReferenceTime(attitude.usec);
+                lastAttitude = time;
                 roll = QGC::limitAngleToPMPIf(attitude.roll);
                 pitch = QGC::limitAngleToPMPIf(attitude.pitch);
                 yaw = QGC::limitAngleToPMPIf(attitude.yaw);
@@ -1252,8 +1258,65 @@ void UAS::startPressureCalibration()
     sendMessage(msg);
 }
 
+quint64 UAS::getUnixReferenceTime(quint64 time)
+{
+    // Same as getUnixTime, but does not react to attitudeStamped mode
+    if (time == 0)
+    {
+        //        qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
+        return QGC::groundTimeMilliseconds();
+    }
+    // Check if time is smaller than 40 years,
+    // assuming no system without Unix timestamp
+    // runs longer than 40 years continuously without
+    // reboot. In worst case this will add/subtract the
+    // communication delay between GCS and MAV,
+    // it will never alter the timestamp in a safety
+    // critical way.
+    //
+    // Calculation:
+    // 40 years
+    // 365 days
+    // 24 hours
+    // 60 minutes
+    // 60 seconds
+    // 1000 milliseconds
+    // 1000 microseconds
+#ifndef _MSC_VER
+    else if (time < 1261440000000000LLU)
+#else
+        else if (time < 1261440000000000)
+#endif
+        {
+        //        qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
+        if (onboardTimeOffset == 0)
+        {
+            onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
+        }
+        return time/1000 + onboardTimeOffset;
+    }
+    else
+    {
+        // Time is not zero and larger than 40 years -> has to be
+        // a Unix epoch timestamp. Do nothing.
+        return time/1000;
+    }
+}
+
+/**
+ * @warning If attitudeStamped is enabled, this function will not actually return the precise time stamp
+ *          of this measurement augmented to UNIX time, but will MOVE the timestamp IN TIME to match
+ *          the last measured attitude. There is no reason why one would want this, except for
+ *          system setups where the onboard clock is not present or broken and datasets should
+ *          be collected that are still roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED
+ *          RUINS THE SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL!
+ */
 quint64 UAS::getUnixTime(quint64 time)
 {
+    if (attitudeStamped)
+    {
+        return lastAttitude;
+    }
     if (time == 0)
     {
         //        qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();

src/uas/UAS.h

@@ -204,6 +204,8 @@ protected: //COMMENTS FOR TEST UNIT
     QGCUASParamManager* paramManager; ///< Parameter manager class
     QString shortStateText;     ///< Short textual state description
     QString shortModeText;      ///< Short textual mode description
+    bool attitudeStamped;       ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV
+    quint64 lastAttitude;       ///< Timestamp of last attitude measurement
 
 public:
     /** @brief Set the current battery type */
@@ -403,6 +405,8 @@ signals:
 protected:
     /** @brief Get the UNIX timestamp in milliseconds */
     quint64 getUnixTime(quint64 time=0);
+    /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */
+    quint64 getUnixReferenceTime(quint64 time);
 
 protected slots:
     /** @brief Write settings to disk */

src/ui/uas/UASView.cc

@@ -602,6 +602,8 @@ void UASView::refresh()
             m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name()));
             QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name());
             m_ui->uasViewFrame->setStyleSheet(style);
+
+            refreshTimer->setInterval(errorUpdateInterval);
         }
         iconIsRed = !iconIsRed;
     } else {
@@ -609,10 +611,11 @@ void UASView::refresh()
         {
             // Fade heartbeat icon
             // Make color darker
-            heartbeatColor = heartbeatColor.darker(150);
+            heartbeatColor = heartbeatColor.darker(210);
 
             //m_ui->heartbeatIcon->setAutoFillBackground(true);
             m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name()));
+            refreshTimer->setInterval(updateInterval);
         }
     }
     //setUpdatesEnabled(true);

src/ui/uas/UASView.h

@@ -122,7 +122,8 @@ protected:
     QAction* selectAction;
     QAction* selectAirframeAction;
     QAction* setBatterySpecsAction;
-    static const int updateInterval = 700;
+    static const int updateInterval = 800;
+    static const int errorUpdateInterval = 200;
     bool lowPowerModeEnabled; ///< Low power mode reduces update rates