From 3536f1af9d5b9fd5696a0a9a4de957aee5f4b0aa Mon Sep 17 00:00:00 2001 From: Pierre TILAK Date: Wed, 21 Aug 2019 14:15:01 +1200 Subject: [PATCH] Add oucoming messages in log To have both order from the GCS and drone messages in logs So that it's possible to analyze the complete exchange between QGC and the drone. - Add a signal emitted by writeBytes of each LinkInterface - Connect this signal to MAVLinkProtocol::logSentBytes in LinkManager - MAVLinkProtocol::logSentBytes will parse and log messages sent from QGC --- src/comm/LinkInterface.h | 10 ++++++++ src/comm/LinkManager.cc | 1 + src/comm/MAVLinkProtocol.cc | 46 +++++++++++++++++++++++++++++++++++++ src/comm/MAVLinkProtocol.h | 3 +++ src/comm/TCPLink.cc | 1 + src/comm/UDPLink.cc | 1 + 6 files changed, 62 insertions(+) diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index e04ecc4ffd..2af1f66ce8 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -181,6 +181,16 @@ signals: */ void bytesReceived(LinkInterface* link, QByteArray data); + /** + * @brief New data has been sent + * * + * The new data is contained in the QByteArray data. + * The data is logged into telemetry logging system + * + * @param data the new bytes + */ + void bytesSent(LinkInterface* link, QByteArray data); + /** * @brief This signal is emitted instantly when the link is connected **/ diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index caa4d09230..7ca89eb176 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -199,6 +199,7 @@ void LinkManager::_addLink(LinkInterface* link) connect(link, &LinkInterface::communicationError, _app, &QGCApplication::criticalMessageBoxOnMainThread); connect(link, &LinkInterface::bytesReceived, _mavlinkProtocol, &MAVLinkProtocol::receiveBytes); + connect(link, &LinkInterface::bytesSent, _mavlinkProtocol, &MAVLinkProtocol::logSentBytes); _mavlinkProtocol->resetMetadataForLink(link); _mavlinkProtocol->setVersion(_mavlinkProtocol->getCurrentVersion()); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index f5a52d11bb..e1a159dbaf 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -159,6 +159,52 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link) link->setDecodedFirstMavlinkPacket(false); } +/** + * This method parses all outcoming bytes and log a MAVLink packet. + * @param link The interface to read from + * @see LinkInterface + **/ + +void MAVLinkProtocol::logSentBytes(LinkInterface* link, QByteArray b){ + + uint8_t mavlinkChannel = link->mavlinkChannel(); + static mavlink_message_t _sent_message; + + for (int position = 0; position < b.size(); position++) { + + if(mavlink_parse_char(mavlinkChannel, static_cast(b[position]), &_sent_message, &_status)){ + + 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(QDateTime::currentMSecsSinceEpoch() * 1000); + qToBigEndian(time, buf); + + // Then write the message to the buffer + int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &_sent_message); + + // Determine how many bytes were written by adding the timestamp size to the message size + len += sizeof(quint64); + + // Now write this timestamp/message pair to the log. + QByteArray b(reinterpret_cast(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; + } + } + } + } + +} + /** * This method parses all incoming bytes and constructs a MAVLink packet. * It can handle multiple links in parallel, as each link has it's own buffer/ diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 7614ee658b..06a155ac32 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -81,6 +81,9 @@ public: public slots: /** @brief Receive bytes from a communication interface */ void receiveBytes(LinkInterface* link, QByteArray b); + + /** @brief Log bytes sent from a communication interface */ + void logSentBytes(LinkInterface* link, QByteArray b); /** @brief Set the system id of this application */ void setSystemId(int id); diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc index dddbf76cb6..82dd895ca8 100644 --- a/src/comm/TCPLink.cc +++ b/src/comm/TCPLink.cc @@ -81,6 +81,7 @@ void TCPLink::_writeBytes(const QByteArray data) if (_socket) { _socket->write(data); + emit bytesSent(this, data); _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); } } diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 1642b9d92f..19db308570 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -169,6 +169,7 @@ void UDPLink::_writeBytes(const QByteArray data) if (!_socket) { return; } + emit bytesSent(this, data); // Send to all manually targeted systems for(UDPCLient* target: _udpConfig->targetHosts()) { // Skip it if it's part of the session clients below -- GitLab