Browse Source

Simplify logSendBytes

Remove the use of mavlink_parse_char which distorts channel stats
QGC4.4
Pierre TILAK 6 years ago
parent
commit
babe2b7c42
  1. 40
      src/comm/MAVLinkProtocol.cc

40
src/comm/MAVLinkProtocol.cc

@ -167,40 +167,24 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link) @@ -167,40 +167,24 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link)
void MAVLinkProtocol::logSentBytes(LinkInterface* link, QByteArray b){
uint8_t mavlinkChannel = link->mavlinkChannel();
static mavlink_message_t _sent_message;
uint8_t bytes_time[sizeof(quint64)];
for (int position = 0; position < b.size(); position++) {
Q_UNUSED(link);
if(mavlink_parse_char(mavlinkChannel, static_cast<uint8_t>(b[position]), &_sent_message, &_status)){
quint64 time = static_cast<quint64>(QDateTime::currentMSecsSinceEpoch() * 1000);
if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
// Write the uint64 time in microseconds in big endian format before the message.
// This timestamp is saved in UTC time. We are only saving in ms precision because
// getting more than this isn't possible with Qt without a ton of extra code.
quint64 time = static_cast<quint64>(QDateTime::currentMSecsSinceEpoch() * 1000);
qToBigEndian(time, buf);
qToBigEndian(time,bytes_time);
// Then write the message to the buffer
int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &_sent_message);
b.insert(0,QByteArray((const char*)bytes_time,sizeof(bytes_time)));
// Determine how many bytes were written by adding the timestamp size to the message size
len += sizeof(quint64);
int len = b.count();
// Now write this timestamp/message pair to the log.
QByteArray b(reinterpret_cast<const char*>(buf), len);
if(_tempLogFile.write(b) != len)
{
// If there's an error logging data, raise an alert and stop logging.
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("MAVLink Logging failed. Could not write to file %1, logging disabled.").arg(_tempLogFile.fileName()));
_stopLogging();
_logSuspendError = true;
}
}
}
if(_tempLogFile.write(b) != len)
{
// If there's an error logging data, raise an alert and stop logging.
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("MAVLink Logging failed. Could not write to file %1, logging disabled.").arg(_tempLogFile.fileName()));
_stopLogging();
_logSuspendError = true;
}
}

Loading…
Cancel
Save