Skip to content

Q
qgroundcontrol
Commit 0d5754dc authored by pixhawk's avatar pixhawk
Browse files
Options

Fixed logging

parent 9666feaa
Hide whitespace changes
Inline Side-by-side
Showing with 142 additions and 57 deletions
src/uas/PxQuadMAV.cc
View file @ 0d5754dc
......@@ -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
View file @ 0d5754dc
......@@ -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<qint64>(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)
......
src/ui/MAVLinkDecoder.h
View file @ 0d5754dc
......@@ -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
View file @ 0d5754dc
......@@ -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())
{
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment