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

@@ -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
      **/

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());

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<uint8_t>(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<quint64>(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<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;
+                }
+            }
+        }
+    }
+
+}
+
 /**
  * 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/

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);

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());
     }
 }

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