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.h

@@ -64,9 +64,10 @@ 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; }

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h

@@ -45,7 +45,8 @@ 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; }

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.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

@@ -80,7 +80,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.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

@@ -77,6 +77,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/QGCApplication.cc

@@ -161,6 +161,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"
@@ -245,9 +244,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);
@@ -3699,6 +3695,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

@@ -606,6 +606,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;