Merge pull request #7714 from Aeronavics/add_outcoming_log

Add oucoming messages in tlog

src/comm/BluetoothLink.cc

@@ -78,6 +78,7 @@ void BluetoothLink::_writeBytes(const QByteArray bytes)
 {
     if (_targetSocket) {
         if(_targetSocket->write(bytes) > 0) {
+            emit bytesSent(this, bytes);
             _logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch());
         } else {
             qWarning() << "Bluetooth write error";

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,38 @@ 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 bytes_time[sizeof(quint64)];
+
+    Q_UNUSED(link);
+    if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {
+
+        quint64 time = static_cast<quint64>(QDateTime::currentMSecsSinceEpoch() * 1000);
+
+        qToBigEndian(time,bytes_time);
+
+        b.insert(0,QByteArray((const char*)bytes_time,sizeof(bytes_time)));
+
+        int len = b.count();
+
+        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/SerialLink.cc

@@ -84,6 +84,7 @@ bool SerialLink::_isBootloader()
 void SerialLink::_writeBytes(const QByteArray data)
 {
     if(_port && _port->isOpen()) {
+        emit bytesSent(this, data);
         _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch());
         _port->write(data);
     } else {

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