Merge branch 'master' into FlightTrajectory

ChangeLog.md

@@ -7,6 +7,7 @@ Note: This file only contains high level features or important fixes.
 ### 3.6.0 - Daily Build
 
 * More performant flight path display algorithm. Mobile builds no longer show limited path length.
+* ArduCopter/Rover: Add support for Follow Me
 * ArduPilot: Add Motor Test vehicle setup page
 * Compass Instrument: Add indicators for Home, COG and Next Waypoint headings.
 * Log Replay: Support changing speed of playback

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -1086,3 +1086,47 @@ void APMFirmwarePlugin::_handleRCChannelsRaw(Vehicle* vehicle, mavlink_message_t
                 &channels);
 }
 
+void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities)
+{
+    if (!vehicle->homePosition().isValid()) {
+        static bool sentOnce = false;
+        if (!sentOnce) {
+            sentOnce = true;
+            qgcApp()->showMessage(tr("Follow failed: Home position not set."));
+        }
+        return;
+    }
+
+    if (!(estimationCapabilities & (FollowMe::POS | FollowMe::VEL))) {
+        static bool sentOnce = false;
+        if (!sentOnce) {
+            sentOnce = true;
+            qWarning() << "APMFirmwarePlugin::_sendGCSMotionReport estimateCapabilities" << estimationCapabilities;
+            qgcApp()->showMessage(tr("Follow failed: Ground station cannot provide required position information."));
+        }
+        return;
+    }
+
+    MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
+
+    mavlink_global_position_int_t globalPositionInt;
+    memset(&globalPositionInt, 0, sizeof(globalPositionInt));
+
+    globalPositionInt.time_boot_ms =    static_cast<uint32_t>(qgcApp()->msecsSinceBoot());
+    globalPositionInt.lat =             motionReport.lat_int;
+    globalPositionInt.lon =             motionReport.lon_int;
+    globalPositionInt.alt =             static_cast<int32_t>(motionReport.altMetersAMSL * 1000);                                        // mm
+    globalPositionInt.relative_alt =    static_cast<int32_t>((motionReport.altMetersAMSL - vehicle->homePosition().altitude()) * 1000); // mm
+    globalPositionInt.vx =              static_cast<int16_t>(motionReport.vxMetersPerSec * 100);                                        // cm/sec
+    globalPositionInt.vy =              static_cast<int16_t>(motionReport.vyMetersPerSec * 100);                                        // cm/sec
+    globalPositionInt.vy =              static_cast<int16_t>(motionReport.vzMetersPerSec * 100);                                        // cm/sec
+    globalPositionInt.hdg =             UINT16_MAX;
+
+    mavlink_message_t message;
+    mavlink_msg_global_position_int_encode_chan(static_cast<uint8_t>(mavlinkProtocol->getSystemId()),
+                                          static_cast<uint8_t>(mavlinkProtocol->getComponentId()),
+                                          vehicle->priorityLink()->mavlinkChannel(),
+                                          &message,
+                                          &globalPositionInt);
+    vehicle->sendMessageOnLink(vehicle->priorityLink(), message);
+}

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -17,6 +17,7 @@
 #include "FirmwarePlugin.h"
 #include "QGCLoggingCategory.h"
 #include "APMParameterMetaData.h"
+#include "FollowMe.h"
 
 #include <QAbstractSocket>
 
@@ -109,7 +110,9 @@ public:
 protected:
     /// All access to singleton is through stack specific implementation
     APMFirmwarePlugin(void);
-    void setSupportedModes(QList<APMCustomMode> supportedModes);
+
+    void setSupportedModes  (QList<APMCustomMode> supportedModes);
+    void _sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities);
 
     bool                _coaxialMotors;
 

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -43,7 +43,7 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) :
         { GUIDED_NOGPS, "Guided No GPS"},
         { SMART_RTL,    "Smart RTL"},
         { FLOWHOLD,     "Flow Hold" },
-        { FOLLOW,       "Follow Vehicle" },
+        { FOLLOW,       "Follow" },
         { ZIGZAG,       "ZigZag" },
     });
 }
@@ -136,3 +136,7 @@ bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle*
     return true;
 }
 
+void ArduCopterFirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities)
+{
+    _sendGCSMotionReport(vehicle, motionReport, estimatationCapabilities);
+}

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h

@@ -64,12 +64,14 @@ public:
     bool    multiRotorCoaxialMotors             (Vehicle* vehicle) final;
     bool    multiRotorXConfig                   (Vehicle* vehicle) final;
     QString offlineEditingParamFile             (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); }
-    QString pauseFlightMode                     (void) const override { return QString("Brake"); }
-    QString landFlightMode                      (void) const override { return QString("Land"); }
-    QString takeControlFlightMode               (void) const override { return QString("Loiter"); }
+    QString pauseFlightMode                     (void) const override { return QStringLiteral("Brake"); }
+    QString landFlightMode                      (void) const override { return QStringLiteral("Land"); }
+    QString takeControlFlightMode               (void) const override { return QStringLiteral("Loiter"); }
+    QString followFlightMode                    (void) const override { return QStringLiteral("Follow"); }
     bool    vehicleYawsToNextWaypointInMission  (const Vehicle* vehicle) const override;
     QString autoDisarmParameter                 (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); }
     bool    supportsSmartRTL                    (void) const override { return true; }
+    void    sendGCSMotionReport                 (Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) override;
 
 private:
     static bool _remapParamNameIntialized;

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc

@@ -77,3 +77,8 @@ bool ArduRoverFirmwarePlugin::supportsNegativeThrust(Vehicle* /*vehicle*/)
 {
     return true;
 }
+
+void ArduRoverFirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities)
+{
+    _sendGCSMotionReport(vehicle, motionReport, estimatationCapabilities);
+}

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h

@@ -45,13 +45,15 @@ public:
     ArduRoverFirmwarePlugin(void);
 
     // Overrides from FirmwarePlugin
-    QString pauseFlightMode                         (void) const override { return QString("Hold"); }
+    QString pauseFlightMode                         (void) const override { return QStringLiteral("Hold"); }
+    QString followFlightMode                        (void) const override { return QStringLiteral("Follow"); }
     void    guidedModeChangeAltitude                (Vehicle* vehicle, double altitudeChange) final;
     int     remapParamNameHigestMinorVersionNumber  (int majorVersionNumber) const final;
     const   FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
     bool    supportsNegativeThrust                  (Vehicle *) final;
     bool    supportsSmartRTL                        (void) const override { return true; }
     QString offlineEditingParamFile                 (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Rover.OfflineEditing.params"); }
+    void    sendGCSMotionReport                     (Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) override;
 
 private:
     static bool _remapParamNameIntialized;

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -909,3 +909,28 @@ QString FirmwarePlugin::gotoFlightMode(void) const
 {
     return QString();
 }
+
+void FirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities)
+{
+    MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
+
+    mavlink_follow_target_t follow_target = {};
+
+    follow_target.timestamp =           qgcApp()->msecsSinceBoot();
+    follow_target.est_capabilities =    estimationCapabilities;
+    follow_target.position_cov[0] =     static_cast<float>(motionReport.pos_std_dev[0]);
+    follow_target.position_cov[2] =     static_cast<float>(motionReport.pos_std_dev[2]);
+    follow_target.alt =                 static_cast<float>(motionReport.altMetersAMSL);
+    follow_target.lat =                 motionReport.lat_int;
+    follow_target.lon =                 motionReport.lon_int;
+    follow_target.vel[0] =              static_cast<float>(motionReport.vxMetersPerSec);
+    follow_target.vel[1] =              static_cast<float>(motionReport.vyMetersPerSec);
+
+    mavlink_message_t message;
+    mavlink_msg_follow_target_encode_chan(static_cast<uint8_t>(mavlinkProtocol->getSystemId()),
+                                          static_cast<uint8_t>(mavlinkProtocol->getComponentId()),
+                                          vehicle->priorityLink()->mavlinkChannel(),
+                                          &message,
+                                          &follow_target);
+    vehicle->sendMessageOnLink(vehicle->priorityLink(), message);
+}

src/FirmwarePlugin/FirmwarePlugin.h

@@ -19,6 +19,7 @@
 #include "AutoPilotPlugin.h"
 #include "GeoFenceManager.h"
 #include "RallyPointManager.h"
+#include "FollowMe.h"
 
 #include <QList>
 #include <QString>
@@ -122,6 +123,9 @@ public:
     /// Returns the flight mode which the vehicle will be in if it is performing a goto location
     virtual QString gotoFlightMode(void) const;
 
+    /// Returns the flight mode which the vehicle will be for follow me
+    virtual QString followFlightMode(void) const { return QString(); };
+
     /// Set guided flight mode
     virtual void setGuidedMode(Vehicle* vehicle, bool guidedMode);
 
@@ -320,6 +324,9 @@ public:
     ///     @param metaData - MetaData for fact
     virtual void adjustMetaData(MAV_TYPE vehicleType, FactMetaData* metaData) {Q_UNUSED(vehicleType); Q_UNUSED(metaData);}
 
+    /// Sends the appropriate mavlink message for follow me support
+    virtual void sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities);
+
     // FIXME: Hack workaround for non pluginize FollowMe support
     static const QString px4FollowMeFlightMode;
 

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -189,8 +189,6 @@ QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) c
             qWarning() << "Unknown flight mode" << custom_mode;
             return tr("Unknown %1:%2").arg(base_mode).arg(custom_mode);
         }
-    } else {
-        qWarning() << "PX4 Flight Stack flight mode without custom mode enabled?";
     }
 
     return flightMode;

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -43,6 +43,7 @@ public:
     QString             landFlightMode                  (void) const override { return _landingFlightMode; }
     QString             takeControlFlightMode           (void) const override { return _manualFlightMode; }
     QString             gotoFlightMode                  (void) const override { return _holdFlightMode; }
+    QString             followFlightMode                (void) const override { return _followMeFlightMode; };
     void                pauseVehicle                    (Vehicle* vehicle) override;
     void                guidedModeRTL                   (Vehicle* vehicle, bool smartRTL) override;
     void                guidedModeLand                  (Vehicle* vehicle) override;

src/FlightDisplay/FlightDisplayViewMap.qml

@@ -81,7 +81,6 @@ FlightMap {
     // When the user pans the map we stop responding to vehicle coordinate updates until the panRecenterTimer fires
     onUserPannedChanged: {
         if (userPanned) {
-            console.log("user panned")
             userPanned = false
             _disableVehicleTracking = true
             panRecenterTimer.restart()

src/FollowMe/FollowMe.cc

@@ -20,176 +20,162 @@
 #include "AppSettings.h"
 
 FollowMe::FollowMe(QGCApplication* app, QGCToolbox* toolbox)
-    : QGCTool(app, toolbox), estimatation_capabilities(0)
+    : QGCTool(app, toolbox)
 {
-    memset(&_motionReport, 0, sizeof(motionReport_s));
-    runTime.start();
     _gcsMotionReportTimer.setSingleShot(false);
 }
 
 void FollowMe::setToolbox(QGCToolbox* toolbox)
 {
     QGCTool::setToolbox(toolbox);
-    connect(&_gcsMotionReportTimer, &QTimer::timeout, this, &FollowMe::_sendGCSMotionReport);
-    connect(toolbox->settingsManager()->appSettings()->followTarget(), &Fact::rawValueChanged, this, &FollowMe::_settingsChanged);
-    _currentMode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt();
-    if(_currentMode == MODE_ALWAYS) {
-        _enable();
-    }
-}
 
-void FollowMe::followMeHandleManager(const QString&)
-{
-    //-- If we are to change based on current flight mode
-    if(_currentMode == MODE_FOLLOWME) {
-        QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles();
-        for (int i=0; i< vehicles.count(); i++) {
-            Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
-            if (vehicle->px4Firmware() && vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
-                _enable();
-                return;
-            }
-        }
-        _disable();
-    }
+    connect(&_gcsMotionReportTimer,                                     &QTimer::timeout,       this, &FollowMe::_sendGCSMotionReport);
+    connect(toolbox->settingsManager()->appSettings()->followTarget(),  &Fact::rawValueChanged, this, &FollowMe::_settingsChanged);
+
+    _settingsChanged();
 }
 
 void FollowMe::_settingsChanged()
 {
-    uint32_t mode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt();
-    if(_currentMode != mode) {
-        _currentMode = mode;
-        switch (mode) {
-        case MODE_NEVER:
-            if(_gcsMotionReportTimer.isActive()) {
-                _disable();
-            }
-            break;
-        case MODE_ALWAYS:
-            if(!_gcsMotionReportTimer.isActive()) {
-                _enable();
-            }
-            break;
-        case MODE_FOLLOWME:
-            if(!_gcsMotionReportTimer.isActive()) {
-                followMeHandleManager(QString());
-            }
-            break;
-        default:
-            break;
-        }
+    _currentMode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt();
+
+    switch (_currentMode) {
+    case MODE_NEVER:
+        disconnect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded,     this, &FollowMe::_vehicleAdded);
+        disconnect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved,   this, &FollowMe::_vehicleRemoved);
+        _disableFollowSend();
+        break;
+    case MODE_ALWAYS:
+        connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded,    this, &FollowMe::_vehicleAdded);
+        connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved,  this, &FollowMe::_vehicleRemoved);
+        _enableFollowSend();
+        break;
+    case MODE_FOLLOWME:
+        connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded,    this, &FollowMe::_vehicleAdded);
+        connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved,  this, &FollowMe::_vehicleRemoved);
+        _enableIfVehicleInFollow();
+        break;
     }
 }
 
-void FollowMe::_enable()
+void FollowMe::_enableFollowSend()
 {
-    connect(_toolbox->qgcPositionManager(),
-            &QGCPositionManager::positionInfoUpdated,
-            this,
-            &FollowMe::_setGPSLocation);
-    _gcsMotionReportTimer.setInterval(_toolbox->qgcPositionManager()->updateInterval());
-    _gcsMotionReportTimer.start();
+    if (!_gcsMotionReportTimer.isActive()) {
+        _gcsMotionReportTimer.setInterval(qMin(_toolbox->qgcPositionManager()->updateInterval(), 250));
+        _gcsMotionReportTimer.start();
+    }
 }
 
-void FollowMe::_disable()
+void FollowMe::_disableFollowSend()
 {
-    disconnect(_toolbox->qgcPositionManager(),
-               &QGCPositionManager::positionInfoUpdated,
-               this,
-               &FollowMe::_setGPSLocation);
-    _gcsMotionReportTimer.stop();
+    if (_gcsMotionReportTimer.isActive()) {
+        _gcsMotionReportTimer.stop();
+    }
 }
 
-void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo)
+void FollowMe::_sendGCSMotionReport()
 {
+    QGeoPositionInfo    geoPositionInfo =   _toolbox->qgcPositionManager()->geoPositionInfo();
+    QGeoCoordinate      gcsCoordinate =     geoPositionInfo.coordinate();
+
     if (!geoPositionInfo.isValid()) {
-        //-- Invalidate cached coordinates
-        memset(&_motionReport, 0, sizeof(motionReport_s));
-    } else {
-        // get the current location coordinates
+        return;
+    }
 
-        QGeoCoordinate geoCoordinate = geoPositionInfo.coordinate();
+    GCSMotionReport motionReport = {};
+    uint8_t         estimatation_capabilities = 0;
 
-        _motionReport.lat_int = geoCoordinate.latitude()  * 1e7;
-        _motionReport.lon_int = geoCoordinate.longitude() * 1e7;
-        _motionReport.alt     = geoCoordinate.altitude();
+    // get the current location coordinates
 
-        estimatation_capabilities |= (1 << POS);
+    motionReport.lat_int =          static_cast<int>(gcsCoordinate.latitude()  * 1e7);
+    motionReport.lon_int =          static_cast<int>(gcsCoordinate.longitude() * 1e7);
+    motionReport.altMetersAMSL =    gcsCoordinate.altitude();
+    estimatation_capabilities |=    (1 << POS);
 
-        // get the current eph and epv
+    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) == true) {
+        motionReport.headingDegrees = geoPositionInfo.attribute(QGeoPositionInfo::Direction);
+    }
 
-        if(geoPositionInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy) == true) {
-            _motionReport.pos_std_dev[0] = _motionReport.pos_std_dev[1] = geoPositionInfo.attribute(QGeoPositionInfo::HorizontalAccuracy);
-        }
+    // get the current eph and epv
 
-        if(geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy) == true) {
-            _motionReport.pos_std_dev[2] = geoPositionInfo.attribute(QGeoPositionInfo::VerticalAccuracy);
-        }                
+    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) {
+        motionReport.pos_std_dev[0] = motionReport.pos_std_dev[1] = geoPositionInfo.attribute(QGeoPositionInfo::HorizontalAccuracy);
+    }
 
-        // calculate z velocity if it's available
+    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) {
+        motionReport.pos_std_dev[2] = geoPositionInfo.attribute(QGeoPositionInfo::VerticalAccuracy);
+    }
 
-        if(geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalSpeed)) {
-            _motionReport.vz = geoPositionInfo.attribute(QGeoPositionInfo::VerticalSpeed);
-        }
+    // calculate z velocity if it's available
+
+    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalSpeed)) {
+        motionReport.vzMetersPerSec = geoPositionInfo.attribute(QGeoPositionInfo::VerticalSpeed);
+    }
 
-        // calculate x,y velocity if it's available
+    // calculate x,y velocity if it's available
 
-        if((geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction)   == true) &&
-           (geoPositionInfo.hasAttribute(QGeoPositionInfo::GroundSpeed) == true)) {
+    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) && geoPositionInfo.hasAttribute(QGeoPositionInfo::GroundSpeed) == true) {
+        estimatation_capabilities |= (1 << VEL);
 
-            estimatation_capabilities |= (1 << VEL);
+        qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction));
+        qreal velocity  = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
 
-            qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction));
-            qreal velocity  = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
+        motionReport.vxMetersPerSec = cos(direction)*velocity;
+        motionReport.vyMetersPerSec = sin(direction)*velocity;
+    } else {
+        motionReport.vxMetersPerSec = 0;
+        motionReport.vyMetersPerSec = 0;
+    }
 
-            _motionReport.vx = cos(direction)*velocity;
-            _motionReport.vy = sin(direction)*velocity;
+    QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles();
 
-        } else {
-            _motionReport.vx = 0.0f;
-            _motionReport.vy = 0.0f;
+    for (int i=0; i<vehicles->count(); i++) {
+        Vehicle* vehicle = vehicles->value<Vehicle*>(i);
+        if (_currentMode == MODE_ALWAYS || (_currentMode == MODE_FOLLOWME && _isFollowFlightMode(vehicle, vehicle->flightMode()))) {
+            vehicle->firmwarePlugin()->sendGCSMotionReport(vehicle, motionReport, estimatation_capabilities);
         }
     }
 }
 
-void FollowMe::_sendGCSMotionReport()
+double FollowMe::_degreesToRadian(double deg)
+{
+    return deg * M_PI / 180.0;
+}
+
+void FollowMe::_vehicleAdded(Vehicle* vehicle)
 {
+    connect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow);
+    _enableIfVehicleInFollow();
+}
+
+void FollowMe::_vehicleRemoved(Vehicle* vehicle)
+{
+    disconnect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow);
+    _enableIfVehicleInFollow();
+}
 
-    //-- Do we have any real data?
-    if(_motionReport.lat_int == 0 && _motionReport.lon_int == 0 && _motionReport.alt == 0) {
+void FollowMe::_enableIfVehicleInFollow(void)
+{
+    if (_currentMode == MODE_ALWAYS) {
+        // System always enabled
         return;
     }
 
-    QmlObjectListModel & vehicles    = *_toolbox->multiVehicleManager()->vehicles();
-    MAVLinkProtocol* mavlinkProtocol = _toolbox->mavlinkProtocol();
-    mavlink_follow_target_t follow_target;
-    memset(&follow_target, 0, sizeof(follow_target));
-
-    follow_target.timestamp = runTime.nsecsElapsed() * 1e-6;
-    follow_target.est_capabilities = estimatation_capabilities;
-    follow_target.position_cov[0] = _motionReport.pos_std_dev[0];
-    follow_target.position_cov[2] = _motionReport.pos_std_dev[2];
-    follow_target.alt = _motionReport.alt;
-    follow_target.lat = _motionReport.lat_int;
-    follow_target.lon = _motionReport.lon_int;
-    follow_target.vel[0] = _motionReport.vx;
-    follow_target.vel[1] = _motionReport.vy;
-
-    for (int i=0; i< vehicles.count(); i++) {
-        Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
-        if(_currentMode || vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
-            mavlink_message_t message;
-            mavlink_msg_follow_target_encode_chan(mavlinkProtocol->getSystemId(),
-                                                  mavlinkProtocol->getComponentId(),
-                                                  vehicle->priorityLink()->mavlinkChannel(),
-                                                  &message,
-                                                  &follow_target);
-            vehicle->sendMessageOnLink(vehicle->priorityLink(), message);
+    // Any vehicle in follow mode will enable the system
+    QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles();
+
+    for (int i=0; i< vehicles->count(); i++) {
+        Vehicle* vehicle = vehicles->value<Vehicle*>(i);
+        if (_isFollowFlightMode(vehicle, vehicle->flightMode())) {
+            _enableFollowSend();
+            return;
         }
     }
+
+    _disableFollowSend();
 }
 
-double FollowMe::_degreesToRadian(double deg)
+bool FollowMe::_isFollowFlightMode (Vehicle* vehicle, const QString& flightMode)
 {
-    return deg * M_PI / 180.0;
+    return flightMode.compare(vehicle->followFlightMode()) == 0;
 }

src/FollowMe/FollowMe.h

@@ -20,6 +20,8 @@
 #include "QGCToolbox.h"
 #include "MAVLinkProtocol.h"
 
+class Vehicle;
+
 Q_DECLARE_LOGGING_CATEGORY(FollowMeLog)
 
 class FollowMe : public QGCTool
@@ -29,36 +31,18 @@ class FollowMe : public QGCTool
 public:
     FollowMe(QGCApplication* app, QGCToolbox* toolbox);
 
-    void setToolbox(QGCToolbox* toolbox) override;
-
-public slots:
-    void followMeHandleManager  (const QString&);
-
-private slots:
-    void _setGPSLocation        (QGeoPositionInfo geoPositionInfo);
-    void _sendGCSMotionReport   ();
-    void _settingsChanged       ();
-
-private:
-    QElapsedTimer runTime;    
-    QTimer _gcsMotionReportTimer;   // Timer to emit motion reports
-
-    struct motionReport_s {
-        uint32_t timestamp;     // time since boot
-        int32_t lat_int;        // X Position in WGS84 frame in 1e7 * meters
-        int32_t lon_int;        // Y Position in WGS84 frame in 1e7 * meters
-        float alt;              //	Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT
-        float vx;               //	X velocity in NED frame in meter / s
-        float vy;               //	Y velocity in NED frame in meter / s
-        float vz;               //	Z velocity in NED frame in meter / s
-        float afx;              //	X acceleration in NED frame in meter / s^2 or N
-        float afy;              //	Y acceleration in NED frame in meter / s^2 or N
-        float afz;              //	Z acceleration in NED frame in meter / s^2 or N
-        float pos_std_dev[3];   // -1 for unknown
-    } _motionReport;
+    struct GCSMotionReport {
+        int     lat_int;            // X Position in WGS84 frame in 1e7 * meters
+        int     lon_int;            // Y Position in WGS84 frame in 1e7 * meters
+        double  altMetersAMSL;      //	Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT
+        double  headingDegrees;      // Heading in degrees
+        double  vxMetersPerSec;     //	X velocity in NED frame in meter / s
+        double  vyMetersPerSec;     //	Y velocity in NED frame in meter / s
+        double  vzMetersPerSec;     //	Z velocity in NED frame in meter / s
+        double  pos_std_dev[3];     // -1 for unknown
+    };
 
     // Mavlink defined motion reporting capabilities
-
     enum {
         POS = 0,
         VEL = 1,
@@ -66,18 +50,27 @@ private:
         ATT_RATES = 3
     };
 
-    uint8_t estimatation_capabilities;
-
-    void    _disable    ();
-    void    _enable     ();
+    void setToolbox(QGCToolbox* toolbox) override;
 
-    double  _degreesToRadian(double deg);
+private slots:
+    void _sendGCSMotionReport       (void);
+    void _settingsChanged           (void);
+    void _vehicleAdded              (Vehicle* vehicle);
+    void _vehicleRemoved            (Vehicle* vehicle);
+    void _enableIfVehicleInFollow   (void);
 
+private:
     enum {
         MODE_NEVER,
         MODE_ALWAYS,
         MODE_FOLLOWME
     };
 
-    uint32_t _currentMode;
+    void    _disableFollowSend  (void);
+    void    _enableFollowSend   (void);
+    double  _degreesToRadian    (double deg);
+    bool    _isFollowFlightMode (Vehicle* vehicle, const QString& flightMode);
+
+    QTimer      _gcsMotionReportTimer;
+    uint32_t    _currentMode;
 };

src/PositionManager/PositionManager.cpp

@@ -46,12 +46,11 @@ void QGCPositionManager::setToolbox(QGCToolbox *toolbox)
    }
    _simulatedSource = new SimulatedPosition();
 
-   // Enable this to get a simulated target on desktop
-   // if (_defaultSource == nullptr) {
-   //     _defaultSource = _simulatedSource;
-   // }
-
+#if 1
    setPositionSource(QGCPositionSource::InternalGPS);
+#else
+   setPositionSource(QGCPositionManager::Simulated);
+#endif
 }
 
 void QGCPositionManager::setNmeaSourceDevice(QIODevice* device)
@@ -77,6 +76,8 @@ void QGCPositionManager::setNmeaSourceDevice(QIODevice* device)
 
 void QGCPositionManager::_positionUpdated(const QGeoPositionInfo &update)
 {
+    _geoPositionInfo = update;
+
     QGeoCoordinate newGCSPosition = QGeoCoordinate();
     qreal newGCSHeading = update.attribute(QGeoPositionInfo::Direction);
 

src/PositionManager/PositionManager.h

@@ -35,17 +35,16 @@ public:
         NmeaGPS
     };
 
-    QGeoCoordinate gcsPosition(void) { return _gcsPosition; }
+    QGeoCoordinate      gcsPosition         (void) { return _gcsPosition; }
+    qreal               gcsHeading          (void) { return _gcsHeading; }
+    QGeoPositionInfo    geoPositionInfo     (void) const { return _geoPositionInfo; }
+    void                setPositionSource   (QGCPositionSource source);
+    int                 updateInterval      (void) const;
+    void                setNmeaSourceDevice (QIODevice* device);
 
-    qreal gcsHeading() { return _gcsHeading; }
+    // Overrides from QGCTool
+    void setToolbox(QGCToolbox* toolbox) override;
 
-    void setPositionSource(QGCPositionSource source);
-
-    int updateInterval() const;
-
-    void setToolbox(QGCToolbox* toolbox);
-
-    void setNmeaSourceDevice(QIODevice* device);
 
 private slots:
     void _positionUpdated(const QGeoPositionInfo &update);
@@ -57,9 +56,10 @@ signals:
     void positionInfoUpdated(QGeoPositionInfo update);
 
 private:
-    int             _updateInterval;
-    QGeoCoordinate  _gcsPosition;
-    qreal           _gcsHeading;
+    int                 _updateInterval;
+    QGeoPositionInfo    _geoPositionInfo;
+    QGeoCoordinate      _gcsPosition;
+    qreal               _gcsHeading;
 
     QGeoPositionInfoSource*     _currentSource;
     QGeoPositionInfoSource*     _defaultSource;

src/PositionManager/SimulatedPosition.cc

@@ -12,28 +12,30 @@
 #include <QDate>
 
 #include "SimulatedPosition.h"
-
-SimulatedPosition::simulated_motion_s SimulatedPosition::_simulated_motion[5] = {{0,250},{0,0},{0, -250},{-250, 0},{0,0}};
+#include "QGCApplication.h"
+#include "MultiVehicleManager.h"
 
 SimulatedPosition::SimulatedPosition()
-    : QGeoPositionInfoSource(nullptr),
-      lat_int(47.3977420*1e7),
-      lon_int(8.5455941*1e7),
-      _step_cnt(0),
-      _simulate_motion_index(0),
-      _simulate_motion(true),
-      _rotation(0.0F)
+    : QGeoPositionInfoSource(nullptr)
 {
-    QDateTime currentDateTime = QDateTime::currentDateTime();
+    _updateTimer.setSingleShot(false);
+
+    // Initialize position to normal PX4 Gazebo home position
+    _lastPosition.setTimestamp(QDateTime::currentDateTime());
+    _lastPosition.setCoordinate(QGeoCoordinate(47.3977420, 8.5455941, 488));
+    _lastPosition.setAttribute(QGeoPositionInfo::Attribute::Direction, _heading);
+    _lastPosition.setAttribute(QGeoPositionInfo::Attribute::GroundSpeed, _horizontalVelocityMetersPerSec);
+    _lastPosition.setAttribute(QGeoPositionInfo::Attribute::VerticalSpeed, _verticalVelocityMetersPerSec);
 
-    qsrand(currentDateTime.toTime_t());
+    // When a vehicle shows up we switch location to the vehicle home position
+    connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &SimulatedPosition::_vehicleAdded);
 
-    connect(&update_timer, &QTimer::timeout, this, &SimulatedPosition::updatePosition);
+    connect(&_updateTimer, &QTimer::timeout, this, &SimulatedPosition::_updatePosition);
 }
 
 QGeoPositionInfo SimulatedPosition::lastKnownPosition(bool /*fromSatellitePositioningMethodsOnly*/) const
 {
-    return lastPosition;
+    return _lastPosition;
 }
 
 SimulatedPosition::PositioningMethods SimulatedPosition::supportedPositioningMethods() const
@@ -41,24 +43,14 @@ SimulatedPosition::PositioningMethods SimulatedPosition::supportedPositioningMet
     return AllPositioningMethods;
 }
 
-int SimulatedPosition::minimumUpdateInterval() const
+void SimulatedPosition::startUpdates(void)
 {
-    return 1000;
+    _updateTimer.start(qMax(updateInterval(), minimumUpdateInterval()));
 }
 
-void SimulatedPosition::startUpdates()
-{    
-    int interval = updateInterval();
-    if (interval < minimumUpdateInterval())
-        interval = minimumUpdateInterval();
-
-    update_timer.setSingleShot(false);
-    update_timer.start(interval);
-}
-
-void SimulatedPosition::stopUpdates()
+void SimulatedPosition::stopUpdates(void)
 {
-    update_timer.stop();
+    _updateTimer.stop();
 }
 
 void SimulatedPosition::requestUpdate(int /*timeout*/)
@@ -66,53 +58,17 @@ void SimulatedPosition::requestUpdate(int /*timeout*/)
     emit updateTimeout();
 }
 
-int SimulatedPosition::getRandomNumber(int size)
-{
-    if(size == 0) {
-        return 0;
-    }
-
-    int num = (qrand()%2 > 1) ? -1 : 1;
-
-    return num*qrand()%size;
-}
-
-void SimulatedPosition::updatePosition()
+void SimulatedPosition::_updatePosition(void)
 {
-    int32_t lat_mov = 0;
-    int32_t lon_mov = 0;
-
-    _rotation += (float) .1;
-
-    if(!(_step_cnt++%30)) {
-        _simulate_motion_index++;
-        if(_simulate_motion_index > 4) {
-            _simulate_motion_index = 0;
-        }
-    }
+    int intervalMsecs = _updateTimer.interval();
 
-    lat_mov = _simulated_motion[_simulate_motion_index].lat;
-    lon_mov = _simulated_motion[_simulate_motion_index].lon*sin(_rotation);
+    QGeoCoordinate  coord =                 _lastPosition.coordinate();
+    double          horizontalDistance =    _horizontalVelocityMetersPerSec * (1000.0 / static_cast<double>(intervalMsecs));
+    double          verticalDistance =      _verticalVelocityMetersPerSec * (1000.0 / static_cast<double>(intervalMsecs));
 
-    lon_int += lat_mov;
-    lat_int += lon_mov;
+    _lastPosition.setCoordinate(coord.atDistanceAndAzimuth(horizontalDistance, _heading, verticalDistance));
 
-    double latitude = ((double)  (lat_int + getRandomNumber(250)))*1e-7;
-    double longitude = ((double) (lon_int + getRandomNumber(250)))*1e-7;
-
-    QDateTime timestamp = QDateTime::currentDateTime();
-
-    QGeoCoordinate position(latitude, longitude);
-    QGeoPositionInfo info(position, timestamp);
-
-    if(lat_mov || lon_mov) {
-        info.setAttribute(QGeoPositionInfo::Attribute::Direction, 3.14/2);
-        info.setAttribute(QGeoPositionInfo::Attribute::GroundSpeed, 5);
-    }
-
-    lastPosition = info;
-
-    emit positionUpdated(info);
+    emit positionUpdated(_lastPosition);
 }
 
 QGeoPositionInfoSource::Error SimulatedPosition::error() const
@@ -120,4 +76,21 @@ QGeoPositionInfoSource::Error SimulatedPosition::error() const
     return QGeoPositionInfoSource::NoError;
 }
 
+void SimulatedPosition::_vehicleAdded(Vehicle* vehicle)
+{
+    if (vehicle->homePosition().isValid()) {
+        _lastPosition.setCoordinate(vehicle->homePosition());
+    } else {
+        connect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged);
+    }
+}
+
+void SimulatedPosition::_vehicleHomePositionChanged(QGeoCoordinate homePosition)
+{
+    Vehicle* vehicle = qobject_cast<Vehicle*>(sender());
 
+    if (homePosition.isValid()) {
+        _lastPosition.setCoordinate(homePosition);
+        disconnect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged);
+    }
+}

src/PositionManager/SimulatedPosition.h

@@ -7,13 +7,14 @@
  *
  ****************************************************************************/
 
-
 #pragma once
 
 #include <QtPositioning/qgeopositioninfosource.h>
 #include "QGCToolbox.h"
 #include <QTimer>
 
+class Vehicle;
+
 class SimulatedPosition : public QGeoPositionInfoSource
 {
    Q_OBJECT
@@ -21,42 +22,28 @@ class SimulatedPosition : public QGeoPositionInfoSource
 public:
     SimulatedPosition();
 
-    QGeoPositionInfo lastKnownPosition(bool fromSatellitePositioningMethodsOnly = false) const;
+    QGeoPositionInfo lastKnownPosition(bool fromSatellitePositioningMethodsOnly = false) const override;
 
-    PositioningMethods supportedPositioningMethods() const;
-    int minimumUpdateInterval() const;
-    Error error() const;
+    PositioningMethods  supportedPositioningMethods (void) const override;
+    int                 minimumUpdateInterval       (void) const override { return _updateIntervalMsecs; }
+    Error               error                       (void) const override;
 
 public slots:
-    virtual void startUpdates();
-    virtual void stopUpdates();
-
-    virtual void requestUpdate(int timeout = 5000);
+    void startUpdates   (void) override;
+    void stopUpdates    (void) override;
+    void requestUpdate  (int timeout = 5000) override;
 
 private slots:
-    void updatePosition();
+    void _updatePosition                (void);
+    void _vehicleAdded                  (Vehicle* vehicle);
+    void _vehicleHomePositionChanged    (QGeoCoordinate homePosition);
 
 private:
-    QTimer update_timer;
-
-    QGeoPositionInfo lastPosition;
-
-    // items for simulating QGC movement in jMAVSIM
-
-    int32_t lat_int;
-    int32_t lon_int;
-
-    struct simulated_motion_s {
-        int lon;
-        int lat;
-    };
-
-    static simulated_motion_s _simulated_motion[5];
-
-    int getRandomNumber(int size);
-    int _step_cnt;
-    int _simulate_motion_index;
+    QTimer              _updateTimer;
+    QGeoPositionInfo    _lastPosition;
 
-    bool _simulate_motion;
-    float _rotation;
+    static constexpr int    _updateIntervalMsecs =              1000;
+    static constexpr double _horizontalVelocityMetersPerSec =   0.5;
+    static constexpr double _verticalVelocityMetersPerSec =     0.1;
+    static constexpr double _heading =                          45;
 };

src/QGCApplication.cc

@@ -162,6 +162,8 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting)
     , _runningUnitTests         (unitTesting)
 {
     _app = this;
+    _msecsElapsedTime.start();
+
 #ifdef Q_OS_LINUX
 #ifndef __mobile__
     if (!_runningUnitTests) {

src/QGCApplication.h

@@ -22,6 +22,7 @@
 #include <QApplication>
 #include <QTimer>
 #include <QQmlApplicationEngine>
+#include <QElapsedTimer>
 
 #include "LinkConfiguration.h"
 #include "LinkManager.h"
@@ -98,6 +99,8 @@ public:
     void            setLanguage();
     QQuickItem*     mainRootWindow();
 
+    uint64_t        msecsSinceBoot(void) { return _msecsElapsedTime.elapsed(); }
+
 public slots:
     /// You can connect to this slot to show an information message box from a different thread.
     void informationMessageBoxOnMainThread(const QString& title, const QString& msg);
@@ -192,6 +195,7 @@ private:
     QTranslator         _QGCTranslatorQt;
     QLocale             _locale;
     bool                _error                  = false;
+    QElapsedTimer       _msecsElapsedTime;
 
     static const char* _settingsVersionKey;             ///< Settings key which hold settings version
     static const char* _deleteAllSettingsKey;           ///< If this settings key is set on boot, all settings will be deleted

src/Settings/App.SettingsGroup.json

@@ -212,7 +212,7 @@
     "type":             "uint32",
     "enumStrings":      "Never,Always,When in Follow Me Flight Mode",
     "enumValues":       "0,1,2",
-    "defaultValue":     0
+    "defaultValue":     2
 },
 {
     "name":                 "apmStartMavlinkStreams",

src/Vehicle/Vehicle.cc

@@ -28,7 +28,6 @@
 #include "ParameterManager.h"
 #include "QGCApplication.h"
 #include "QGCImageProvider.h"
-#include "FollowMe.h"
 #include "MissionCommandTree.h"
 #include "QGroundControlQmlGlobal.h"
 #include "SettingsManager.h"
@@ -247,9 +246,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     _commonInit();
     _autopilotPlugin = _firmwarePlugin->autopilotPlugin(this);
 
-    // connect this vehicle to the follow me handle manager
-    connect(this, &Vehicle::flightModeChanged,_toolbox->followMe(), &FollowMe::followMeHandleManager);
-
     // PreArm Error self-destruct timer
     connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout);
     _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs);
@@ -3685,6 +3681,11 @@ QString Vehicle::takeControlFlightMode(void) const
     return _firmwarePlugin->takeControlFlightMode();
 }
 
+QString Vehicle::followFlightMode(void) const
+{
+    return _firmwarePlugin->followFlightMode();
+}
+
 QString Vehicle::vehicleImageOpaque() const
 {
     if(_firmwarePlugin)

src/Vehicle/Vehicle.h

@@ -607,6 +607,7 @@ public:
     Q_PROPERTY(bool                 supportsSmartRTL        READ supportsSmartRTL                                       CONSTANT)
     Q_PROPERTY(QString              landFlightMode          READ landFlightMode                                         CONSTANT)
     Q_PROPERTY(QString              takeControlFlightMode   READ takeControlFlightMode                                  CONSTANT)
+    Q_PROPERTY(QString              followFlightMode        READ followFlightMode                                       CONSTANT)
     Q_PROPERTY(QString              firmwareTypeString      READ firmwareTypeString                                     NOTIFY firmwareTypeChanged)
     Q_PROPERTY(QString              vehicleTypeString       READ vehicleTypeString                                      NOTIFY vehicleTypeChanged)
     Q_PROPERTY(QString              vehicleImageOpaque      READ vehicleImageOpaque                                     CONSTANT)
@@ -942,6 +943,7 @@ public:
     bool            supportsSmartRTL        () const;
     QString         landFlightMode          () const;
     QString         takeControlFlightMode   () const;
+    QString         followFlightMode        () const;
     double          defaultCruiseSpeed      () const { return _defaultCruiseSpeed; }
     double          defaultHoverSpeed       () const { return _defaultHoverSpeed; }
     QString         firmwareTypeString      () const;

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

@@ -182,6 +182,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
      **/
     void 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

@@ -160,6 +160,38 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link)
 }
 
 /**
+ * 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/
  * parsing state machine.

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