Fixed logging

src/uas/PxQuadMAV.cc

@@ -100,19 +100,19 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
             emit processChanged(this->uasId, payload.watchdog_id, payload.process_id, payload.state, (payload.muted == 1) ? true : false, payload.crashes, payload.pid);
         }
         break;
-        case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: {
-            mavlink_vision_position_estimate_t pos;
-            mavlink_msg_vision_position_estimate_decode(&message, &pos);
-            quint64 time = getUnixTime(pos.usec);
-            //emit valueChanged(uasId, "vis. time", pos.usec, time);
-            emit valueChanged(uasId, "vis. roll", "rad", pos.roll, time);
-            emit valueChanged(uasId, "vis. pitch", "rad", pos.pitch, time);
-            emit valueChanged(uasId, "vis. yaw", "rad", pos.yaw, time);
-            emit valueChanged(uasId, "vis. x", "m", pos.x, time);
-            emit valueChanged(uasId, "vis. y", "m", pos.y, time);
-            emit valueChanged(uasId, "vis. z", "m", pos.z, time);
-        }
-        break;
+//        case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: {
+//            mavlink_vision_position_estimate_t pos;
+//            mavlink_msg_vision_position_estimate_decode(&message, &pos);
+//            quint64 time = getUnixTime(pos.usec);
+//            //emit valueChanged(uasId, "vis. time", pos.usec, time);
+//            emit valueChanged(uasId, "vis. roll", "rad", pos.roll, time);
+//            emit valueChanged(uasId, "vis. pitch", "rad", pos.pitch, time);
+//            emit valueChanged(uasId, "vis. yaw", "rad", pos.yaw, time);
+//            emit valueChanged(uasId, "vis. x", "m", pos.x, time);
+//            emit valueChanged(uasId, "vis. y", "m", pos.y, time);
+//            emit valueChanged(uasId, "vis. z", "m", pos.z, time);
+//        }
+//        break;
 //        case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE: {
 //            mavlink_global_vision_position_estimate_t pos;
 //            mavlink_msg_global_vision_position_estimate_decode(&message, &pos);
@@ -126,29 +126,29 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
 //            emit valueChanged(uasId, "glob. vis. z", "m", pos.z, time);
 //        }
 //        break;
-        case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: {
-            mavlink_vicon_position_estimate_t pos;
-            mavlink_msg_vicon_position_estimate_decode(&message, &pos);
-            quint64 time = getUnixTime(pos.usec);
-            //emit valueChanged(uasId, "vis. time", pos.usec, time);
-            emit valueChanged(uasId, "vicon roll", "rad", pos.roll, time);
-            emit valueChanged(uasId, "vicon pitch", "rad", pos.pitch, time);
-            emit valueChanged(uasId, "vicon yaw", "rad", pos.yaw, time);
-            emit valueChanged(uasId, "vicon x", "m", pos.x, time);
-            emit valueChanged(uasId, "vicon y", "m", pos.y, time);
-            emit valueChanged(uasId, "vicon z", "m", pos.z, time);
-            //emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
-        }
-        break;
-        case MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE: {
-            mavlink_vision_speed_estimate_t speed;
-            mavlink_msg_vision_speed_estimate_decode(&message, &speed);
-            quint64 time = getUnixTime(speed.usec);
-            emit valueChanged(uasId, "vis. speed x", "m/s", speed.x, time);
-            emit valueChanged(uasId, "vis. speed y", "m/s", speed.y, time);
-            emit valueChanged(uasId, "vis. speed z", "m/s", speed.z, time);
-        }
-        break;
+//        case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: {
+//            mavlink_vicon_position_estimate_t pos;
+//            mavlink_msg_vicon_position_estimate_decode(&message, &pos);
+//            quint64 time = getUnixTime(pos.usec);
+//            //emit valueChanged(uasId, "vis. time", pos.usec, time);
+//            emit valueChanged(uasId, "vicon roll", "rad", pos.roll, time);
+//            emit valueChanged(uasId, "vicon pitch", "rad", pos.pitch, time);
+//            emit valueChanged(uasId, "vicon yaw", "rad", pos.yaw, time);
+//            emit valueChanged(uasId, "vicon x", "m", pos.x, time);
+//            emit valueChanged(uasId, "vicon y", "m", pos.y, time);
+//            emit valueChanged(uasId, "vicon z", "m", pos.z, time);
+//            //emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
+//        }
+//        break;
+//        case MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE: {
+//            mavlink_vision_speed_estimate_t speed;
+//            mavlink_msg_vision_speed_estimate_decode(&message, &speed);
+//            quint64 time = getUnixTime(speed.usec);
+//            emit valueChanged(uasId, "vis. speed x", "m/s", speed.x, time);
+//            emit valueChanged(uasId, "vis. speed y", "m/s", speed.y, time);
+//            emit valueChanged(uasId, "vis. speed z", "m/s", speed.z, time);
+//        }
+//        break;
 //        case MAVLINK_MSG_ID_CONTROL_STATUS:
 //            {
 //                mavlink_control_status_t status;

src/ui/MAVLinkDecoder.cc

@@ -11,8 +11,11 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
     {
         componentID[i] = -1;
         componentMulti[i] = false;
+        onboardTimeOffset[i] = 0;
     }
 
+
+
     // Fill filter
     messageFilter.insert(MAVLINK_MSG_ID_HEARTBEAT, false);
     messageFilter.insert(MAVLINK_MSG_ID_SYS_STATUS, false);
@@ -41,6 +44,18 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag
     memcpy(receivedMessages+message.msgid, &message, sizeof(mavlink_message_t));
 
     uint8_t msgid = message.msgid;
+
+    // Handle time sync message
+    if (message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME && message.compid == 200)
+    {
+        mavlink_system_time_t timebase;
+        mavlink_msg_system_time_decode(&message, &timebase);
+        onboardTimeOffset[message.sysid] = timebase.time_unix_usec/1000 - timebase.time_boot_ms;
+        onboardToGCSUnixTimeOffsetAndDelay[message.sysid] = static_cast<qint64>(QGC::groundTimeMilliseconds() - timebase.time_unix_usec/1000);
+    }
+    else
+    {
+
         QString messageName("%1 (#%2)");
         messageName = messageName.arg(messageInfo[msgid].name).arg(msgid);
 
@@ -52,26 +67,77 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag
         {
             time = *((quint32*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
         }
-    else if (QString(messageInfo[msgid].fields[fieldid].name) == QString("time_usec") && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT64_T)
+        else if (QString(messageInfo[msgid].fields[fieldid].name).contains("usec") && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT64_T)
         {
             time = *((quint64*)(m+messageInfo[msgid].fields[fieldid].wire_offset));
+            time = time/1000; // Scale to milliseconds
         }
         else
         {
             // First value is not time, send out value 0
-        emitFieldValue(&message, fieldid, time);
+            emitFieldValue(&message, fieldid, getUnixTimeFromMs(message.sysid, 0));
         }
 
+        // Align time to global time
+        time = getUnixTimeFromMs(message.sysid, time);
+
         // Send out field values from 1..n
         for (unsigned int i = 1; i < messageInfo[msgid].num_fields; ++i)
         {
             emitFieldValue(&message, i, time);
         }
+    }
 
     // Send out combined math expressions
     // FIXME XXX TODO
 }
 
+quint64 MAVLinkDecoder::getUnixTimeFromMs(int systemID, quint64 time)
+{
+    quint64 ret = 0;
+    if (time == 0)
+    {
+        ret = QGC::groundTimeMilliseconds() - onboardToGCSUnixTimeOffsetAndDelay[systemID];
+    }
+    // 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 < 1261440000000LLU)
+#else
+    else if (time < 1261440000000)
+#endif
+    {
+        if (onboardTimeOffset[systemID] == 0 || time < (firstOnboardTime[systemID]-2000))
+        {
+            firstOnboardTime[systemID] = time;
+            onboardTimeOffset[systemID] = QGC::groundTimeMilliseconds() - time;
+        }
+        ret = time + onboardTimeOffset[systemID];
+    }
+    else
+    {
+        // Time is not zero and larger than 40 years -> has to be
+        // a Unix epoch timestamp. Do nothing.
+        ret = time;
+    }
+
+    return ret;
+}
+
 void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 time)
 {
     bool multiComponentSourceDetected = false;
@@ -102,7 +168,20 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
     uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8;
     QString name("%1.%2");
     QString unit("");
+
+    // Debug vector messages
+    if (msgid == MAVLINK_MSG_ID_DEBUG_VECT)
+    {
+        mavlink_debug_vect_t debug;
+        mavlink_msg_debug_vect_decode(msg, &debug);
+        name = name.arg(QString(debug.name), fieldName);
+        time = debug.time_usec / 1000;
+    }
+    else
+    {
         name = name.arg(messageInfo[msgid].name, fieldName);
+    }
+
     if (multiComponentSourceDetected) name.prepend(QString("C%1:").arg(msg->compid));
     name.prepend(QString("M%1:").arg(msg->sysid));
     switch (messageInfo[msgid].fields[fieldid].type)

src/ui/MAVLinkDecoder.h

@@ -25,6 +25,8 @@ public slots:
 protected:
     /** @brief Emit the value of one message field */
     void emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 time);
+    /** @brief Shift a timestamp in Unix time if necessary */
+    quint64 getUnixTimeFromMs(int systemID, quint64 time);
 
     mavlink_message_t receivedMessages[256]; ///< Available / known messages
     mavlink_message_info_t messageInfo[256]; ///< Message information
@@ -32,6 +34,9 @@ protected:
     QMap<uint16_t, bool> textMessageFilter;           ///< Message/field names not to emit in text mode
     int componentID[256];                             ///< Multi component detection
     bool componentMulti[256];                         ///< Multi components detected
+    quint64 onboardTimeOffset[256];                   ///< Offset of onboard time from Unix epoch (of the receiving GCS)
+    qint64 onboardToGCSUnixTimeOffsetAndDelay[256];   ///< Offset of onboard time and GCS Unix time
+    quint64 firstOnboardTime[256];                    ///< First seen onboard time
 
 };
 

src/ui/QGCDataPlot2D.cc

@@ -470,7 +470,8 @@ void QGCDataPlot2D::loadCsvLog(QString file, QString xAxisName, QString yAxisFil
 
     // Read data
 
-    double x,y;
+    double x = 0;
+    double y = 0;
 
     while (!in.atEnd())
     {