zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Fixed logging

QGC4.4
pixhawk 13 years ago
parent
9666feaab1
commit
0d5754dcb8
4 changed files with 142 additions and 57 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 72
      src/uas/PxQuadMAV.cc
  2. 119
      src/ui/MAVLinkDecoder.cc
  3. 5
      src/ui/MAVLinkDecoder.h
  4. 3
      src/ui/QGCDataPlot2D.cc

72
src/uas/PxQuadMAV.cc
Unescape View File

@ -100,19 +100,19 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -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) @@ -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;

119
src/ui/MAVLinkDecoder.cc
Unescape View File

@ -11,8 +11,11 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) : @@ -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 @@ -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 @@ -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)

5
src/ui/MAVLinkDecoder.h
Unescape View File

@ -25,6 +25,8 @@ public slots: @@ -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: @@ -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
};

3
src/ui/QGCDataPlot2D.cc
Unescape View File

@ -470,7 +470,8 @@ void QGCDataPlot2D::loadCsvLog(QString file, QString xAxisName, QString yAxisFil @@ -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())
{

Write Preview
Loading…
Cancel
Save