Merge pull request #7822 from DonLakeFlyer/FollowMe

Follow Me work

ChangeLog.md

@@ -6,6 +6,7 @@ Note: This file only contains high level features or important fixes.
 
 ### 3.6.0 - Daily Build
 
+* 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

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

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

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