diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 80b11a81e0861564ef3c521ddc15bc52d7af68dd..cc72fe32a1dce7c8029e68055f27e55c3ccdf5ed 100644 --- a/src/uas/PxQuadMAV.cc +++ b/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; diff --git a/src/ui/MAVLinkDecoder.cc b/src/ui/MAVLinkDecoder.cc index 082b71f93d5d9cd50545437a6dd54181453fd68e..aa077ae0d9fd4fdd2a37ffb35eb2741da5392948 100644 --- a/src/ui/MAVLinkDecoder.cc +++ b/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,37 +44,100 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag memcpy(receivedMessages+message.msgid, &message, sizeof(mavlink_message_t)); uint8_t msgid = message.msgid; - QString messageName("%1 (#%2)"); - messageName = messageName.arg(messageInfo[msgid].name).arg(msgid); - // See if first value is a time value - quint64 time = 0; - uint8_t fieldid = 0; - uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8; - if (QString(messageInfo[msgid].fields[fieldid].name) == QString("time_boot_ms") && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT32_T) - { - 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) + // Handle time sync message + if (message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME && message.compid == 200) { - time = *((quint64*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + 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(QGC::groundTimeMilliseconds() - timebase.time_unix_usec/1000); } else { - // First value is not time, send out value 0 - emitFieldValue(&message, fieldid, time); - } - // Send out field values from 1..n - for (unsigned int i = 1; i < messageInfo[msgid].num_fields; ++i) - { - emitFieldValue(&message, i, time); + QString messageName("%1 (#%2)"); + messageName = messageName.arg(messageInfo[msgid].name).arg(msgid); + + // See if first value is a time value + quint64 time = 0; + uint8_t fieldid = 0; + uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8; + if (QString(messageInfo[msgid].fields[fieldid].name) == QString("time_boot_ms") && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT32_T) + { + time = *((quint32*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + } + 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, 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(""); - name = name.arg(messageInfo[msgid].name, fieldName); + + // 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) diff --git a/src/ui/MAVLinkDecoder.h b/src/ui/MAVLinkDecoder.h index 904b7d55424fff676af6efbaac415dbaa465b2eb..c0c570b2c2479dbeaaf84d09ec1a0f40a94cd0c7 100644 --- a/src/ui/MAVLinkDecoder.h +++ b/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 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 }; diff --git a/src/ui/QGCDataPlot2D.cc b/src/ui/QGCDataPlot2D.cc index 955e36bb9eb5073895dfb672c30f82b75c7813a8..4d44fa82499bdd6db70b3a10491e6a29b05342b7 100644 --- a/src/ui/QGCDataPlot2D.cc +++ b/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()) {