diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc
index a5d76f7f831..9ee688a589b 100644
--- a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc
+++ b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc
@@ -25,6 +25,11 @@
 #include "ESP8266Component.h"
 #include "APMHeliComponent.h"
 #include "APMRemoteSupportComponent.h"
+#ifdef QT_DEBUG
+#include "APMFollowComponent.h"
+#include "ArduCopterFirmwarePlugin.h"
+#include "ArduRoverFirmwarePlugin.h"
+#endif
 #include "QGCApplication.h"
 #include "ParameterManager.h"
 
@@ -50,10 +55,6 @@ APMAutoPilotPlugin::APMAutoPilotPlugin(Vehicle* vehicle, QObject* parent)
     , _esp8266Component         (nullptr)
     , _heliComponent            (nullptr)
     , _apmRemoteSupportComponent(nullptr)
-#if 0
-    // Follow me not ready for Stable
-    , _followComponent          (nullptr)
-#endif
 {
 #if !defined(NO_SERIAL_LINK) && !defined(Q_OS_ANDROID)
     connect(vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &APMAutoPilotPlugin::_checkForBadCubeBlack);
@@ -104,14 +105,12 @@ const QVariantList& APMAutoPilotPlugin::vehicleComponents(void)
             _safetyComponent->setupTriggerSignals();
             _components.append(QVariant::fromValue((VehicleComponent*)_safetyComponent));
 
-#if 0
-    // Follow me not ready for Stable
-
+#ifdef QT_DEBUG
             if ((qobject_cast<ArduCopterFirmwarePlugin*>(_vehicle->firmwarePlugin()) || qobject_cast<ArduRoverFirmwarePlugin*>(_vehicle->firmwarePlugin())) &&
                     _vehicle->parameterManager()->parameterExists(-1, QStringLiteral("FOLL_ENABLE"))) {
                 _followComponent = new APMFollowComponent(_vehicle, this);
                 _followComponent->setupTriggerSignals();
-                _components.append(QVariant::fromValue((VehicleComponent*)_followComponent));
+                (void) _components.append(QVariant::fromValue(qobject_cast<VehicleComponent*>(_followComponent)));
             }
 #endif
 
diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h
index 9b6190267af..ce804e4d5b3 100644
--- a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h
+++ b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h
@@ -58,10 +58,7 @@ class APMAutoPilotPlugin : public AutoPilotPlugin
     ESP8266Component*           _esp8266Component;
     APMHeliComponent*           _heliComponent;
     APMRemoteSupportComponent*  _apmRemoteSupportComponent;
-#if 0
-    // Follow me not ready for Stable
-    APMFollowComponent*         _followComponent;
-#endif
+    APMFollowComponent *_followComponent = nullptr;
 
 #if !defined(NO_SERIAL_LINK) && !defined(Q_OS_ANDROID)
 private slots:
diff --git a/src/AutoPilotPlugins/APM/APMFollowComponent.FactMetaData.json b/src/AutoPilotPlugins/APM/APMFollowComponent.FactMetaData.json
index 57f471481fe..ac488a8f9ba 100644
--- a/src/AutoPilotPlugins/APM/APMFollowComponent.FactMetaData.json
+++ b/src/AutoPilotPlugins/APM/APMFollowComponent.FactMetaData.json
@@ -1,35 +1,35 @@
 {
-    "version":      1,
-    "fileType":  "FactMetaData",
+    "version": 1,
+    "fileType": "FactMetaData",
     "QGC.MetaData.Facts":
 [
 {
-    "name":             "angle",
+    "name": "angle",
     "shortDesc": "Angle from ground station to vehicle",
-    "type":             "double",
-    "min":              0,
-    "max":              360,
-    "decimalPlaces":    1,
-    "units":            "deg",
-    "default":     45
+    "type": "double",
+    "min": 0,
+    "max": 360,
+    "decimalPlaces": 1,
+    "units": "deg",
+    "default": 45
 },
 {
-    "name":             "distance",
+    "name": "distance",
     "shortDesc": "Horizontal distance from ground station to vehicle",
-    "type":             "double",
-    "min":              0,
-    "decimalPlaces":    1,
-    "units":            "m",
-    "default":     5
+    "type": "double",
+    "min": 0,
+    "decimalPlaces": 1,
+    "units": "m",
+    "default": 5
 },
 {
-    "name":             "height",
+    "name": "height",
     "shortDesc": "Vertical distance from Launch (home) position to vehicle",
-    "type":             "double",
-    "min":              0,
-    "decimalPlaces":    1,
-    "units":            "m",
-    "default":     5
+    "type": "double",
+    "min": 0,
+    "decimalPlaces": 1,
+    "units": "m",
+    "default": 5
 }
 ]
 }
diff --git a/src/AutoPilotPlugins/APM/APMFollowComponent.cc b/src/AutoPilotPlugins/APM/APMFollowComponent.cc
index 89a6b5b1f82..6e04a3215dd 100644
--- a/src/AutoPilotPlugins/APM/APMFollowComponent.cc
+++ b/src/AutoPilotPlugins/APM/APMFollowComponent.cc
@@ -10,33 +10,14 @@
 #include "APMFollowComponent.h"
 #include "APMAutoPilotPlugin.h"
 
-APMFollowComponent::APMFollowComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent)
+APMFollowComponent::APMFollowComponent(Vehicle *vehicle, AutoPilotPlugin *autopilot, QObject *parent)
     : VehicleComponent(vehicle, autopilot, parent),
-    _name(tr("Follow Me"))
+    _name(QStringLiteral("Follow Me"))
 {
+    // qCDebug() << Q_FUNC_INFO << this;
 }
 
-QString APMFollowComponent::name(void) const
+APMFollowComponent::~APMFollowComponent()
 {
-    return _name;
-}
-
-QString APMFollowComponent::description(void) const
-{
-    return tr("Follow Me Setup is used to configure support for the vehicle following the ground station location.");
-}
-
-QString APMFollowComponent::iconResource(void) const
-{
-    return QStringLiteral("/qmlimages/FollowComponentIcon.png");
-}
-
-QUrl APMFollowComponent::setupSource(void) const
-{
-    return QUrl::fromUserInput(QStringLiteral("qrc:/qml/APMFollowComponent.qml"));
-}
-
-QUrl APMFollowComponent::summaryQmlSource(void) const
-{
-    return QUrl::fromUserInput(QStringLiteral("qrc:/qml/APMFollowComponentSummary.qml"));
+    // qCDebug() << Q_FUNC_INFO << this;
 }
diff --git a/src/AutoPilotPlugins/APM/APMFollowComponent.h b/src/AutoPilotPlugins/APM/APMFollowComponent.h
index 25998cd25e8..e2e4aea1fa1 100644
--- a/src/AutoPilotPlugins/APM/APMFollowComponent.h
+++ b/src/AutoPilotPlugins/APM/APMFollowComponent.h
@@ -14,25 +14,23 @@
 class APMFollowComponent : public VehicleComponent
 {
     Q_OBJECT
-    
+
 public:
-    APMFollowComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = nullptr);
-    
-    // Overrides from VehicleComponent
-    QStringList setupCompleteChangedTriggerList(void) const override { return QStringList(); }
-    
-    // Virtuals from VehicleComponent
-    QString name                    (void) const override;
-    QString description             (void) const override;
-    QString iconResource            (void) const override;
-    bool    requiresSetup           (void) const override { return false; }
-    bool    setupComplete           (void) const override { return true; }
-    QUrl    setupSource             (void) const override;
-    QUrl    summaryQmlSource        (void) const override;
-    bool    allowSetupWhileArmed    (void) const override { return true; }
-    bool    allowSetupWhileFlying   (void) const override { return true; }
+    APMFollowComponent(Vehicle *vehicle, AutoPilotPlugin *autopilot, QObject *parent = nullptr);
+    ~APMFollowComponent();
+
+    QString name() const final { return _name; }
+    QString description() const final { return QStringLiteral("Follow Me Setup is used to configure support for the vehicle following the ground station location."); }
+    QString iconResource() const final { return QStringLiteral("/qmlimages/FollowComponentIcon.png"); }
+    bool requiresSetup() const final { return false; }
+    bool setupComplete() const final { return true; }
+    QUrl setupSource() const final { return QUrl::fromUserInput(QStringLiteral("qrc:/qml/APMFollowComponent.qml")); }
+    QUrl summaryQmlSource() const final { return QUrl::fromUserInput(QStringLiteral("qrc:/qml/APMFollowComponentSummary.qml")); }
+    bool allowSetupWhileArmed() const final { return true; }
+    bool allowSetupWhileFlying() const final { return true; }
+
+    QStringList setupCompleteChangedTriggerList() const final { return QStringList(); }
 
 private:
-    const QString   _name;
-    QVariantList    _summaryItems;
+    const QString _name;
 };
diff --git a/src/AutoPilotPlugins/APM/APMFollowComponentController.cc b/src/AutoPilotPlugins/APM/APMFollowComponentController.cc
index b745a219a34..06a1925fc25 100644
--- a/src/AutoPilotPlugins/APM/APMFollowComponentController.cc
+++ b/src/AutoPilotPlugins/APM/APMFollowComponentController.cc
@@ -11,13 +11,19 @@
 #include "ArduRoverFirmwarePlugin.h"
 #include "Vehicle.h"
 
-APMFollowComponentController::APMFollowComponentController(void)
-    : _metaDataMap  (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/APMFollowComponent.FactMetaData.json"), this))
-    , _angleFact    (settingsGroup, _metaDataMap[angleName])
-    , _distanceFact (settingsGroup, _metaDataMap[distanceName])
-    , _heightFact   (settingsGroup, _metaDataMap[heightName])
+APMFollowComponentController::APMFollowComponentController(QObject *parent)
+    : FactPanelController(parent)
+    , _metaDataMap(FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/APMFollowComponent.FactMetaData.json"), this))
+    , _angleFact(new SettingsFact(_settingsGroup, _metaDataMap[_angleName], this))
+    , _distanceFact(new SettingsFact(_settingsGroup, _metaDataMap[_distanceName], this))
+    , _heightFact(new SettingsFact(_settingsGroup, _metaDataMap[_heightName], this))
 {
+    // qCDebug() << Q_FUNC_INFO << this;
+}
 
+APMFollowComponentController::~APMFollowComponentController()
+{
+    // qCDebug() << Q_FUNC_INFO << this;
 }
 
 bool APMFollowComponentController::roverFirmware()
diff --git a/src/AutoPilotPlugins/APM/APMFollowComponentController.h b/src/AutoPilotPlugins/APM/APMFollowComponentController.h
index e481f73d0f3..ade59c00c6d 100644
--- a/src/AutoPilotPlugins/APM/APMFollowComponentController.h
+++ b/src/AutoPilotPlugins/APM/APMFollowComponentController.h
@@ -15,29 +15,30 @@
 class APMFollowComponentController : public FactPanelController
 {
     Q_OBJECT
-    
-public:
-    APMFollowComponentController(void);
 
-    Q_PROPERTY(Fact* angle          READ angleFact      CONSTANT)
-    Q_PROPERTY(Fact* distance       READ distanceFact   CONSTANT)
-    Q_PROPERTY(Fact* height         READ heightFact     CONSTANT)
-    Q_PROPERTY(bool  roverFirmware  READ roverFirmware  CONSTANT)
+    Q_PROPERTY(Fact *angle          READ angleFact      CONSTANT)
+    Q_PROPERTY(Fact *distance       READ distanceFact   CONSTANT)
+    Q_PROPERTY(Fact *height         READ heightFact     CONSTANT)
+    Q_PROPERTY(bool roverFirmware   READ roverFirmware  CONSTANT)
 
-    Fact* angleFact     (void) { return &_angleFact; }
-    Fact* distanceFact  (void) { return &_distanceFact; }
-    Fact* heightFact    (void) { return &_heightFact; }
-    bool  roverFirmware (void);
+public:
+    APMFollowComponentController(QObject *parent = nullptr);
+    ~APMFollowComponentController();
 
-    static constexpr const char* settingsGroup =   "APMFollow";
-    static constexpr const char* angleName =       "angle";
-    static constexpr const char* distanceName =    "distance";
-    static constexpr const char* heightName =      "height";
+    Fact *angleFact() { return _angleFact; }
+    Fact *distanceFact() { return _distanceFact; }
+    Fact *heightFact() { return _heightFact; }
+    bool roverFirmware();
 
 private:
     QMap<QString, FactMetaData*> _metaDataMap;
 
-    SettingsFact _angleFact;
-    SettingsFact _distanceFact;
-    SettingsFact _heightFact;
+    SettingsFact *_angleFact = nullptr;
+    SettingsFact *_distanceFact = nullptr;
+    SettingsFact *_heightFact = nullptr;
+
+    static constexpr const char *_settingsGroup = "APMFollow";
+    static constexpr const char *_angleName = "angle";
+    static constexpr const char *_distanceName = "distance";
+    static constexpr const char *_heightName = "height";
 };
diff --git a/src/AutoPilotPlugins/APM/APMFollowComponentSummary.qml b/src/AutoPilotPlugins/APM/APMFollowComponentSummary.qml
index 4d0c8fdc193..a1482c6d8ee 100644
--- a/src/AutoPilotPlugins/APM/APMFollowComponentSummary.qml
+++ b/src/AutoPilotPlugins/APM/APMFollowComponentSummary.qml
@@ -13,46 +13,77 @@ import QtQuick.Controls
 import QGroundControl.FactSystem
 import QGroundControl.FactControls
 import QGroundControl.Controls
-import QGroundControl.Palette
 
 Item {
-    anchors.fill:   parent
+    anchors.fill: parent
 
     FactPanelController { id: controller; }
 
-    property Fact _batt1Monitor:            controller.getParameterFact(-1, "BATT_MONITOR")
-    property Fact _batt2Monitor:            controller.getParameterFact(-1, "BATT2_MONITOR", false /* reportMissing */)
-    property bool _batt2MonitorAvailable:   controller.parameterExists(-1, "BATT2_MONITOR")
-    property bool _batt1MonitorEnabled:     _batt1Monitor.rawValue !== 0
-    property bool _batt2MonitorEnabled:     _batt2MonitorAvailable && _batt2Monitor.rawValue !== 0
-    property Fact _battCapacity:            controller.getParameterFact(-1, "BATT_CAPACITY", false /* reportMissing */)
-    property Fact _batt2Capacity:           controller.getParameterFact(-1, "BATT2_CAPACITY", false /* reportMissing */)
-    property bool _battCapacityAvailable:   controller.parameterExists(-1, "BATT_CAPACITY")
+    property Fact _followEnabled: controller.getParameterFact(-1, "FOLL_ENABLE")
+    property bool _followParamsAvailable: controller.parameterExists(-1, "FOLL_SYSID")
+    property Fact _followDistanceMax: controller.getParameterFact(-1, "FOLL_DIST_MAX", false /* reportMissing */)
+    property Fact _followSysId: controller.getParameterFact(-1, "FOLL_SYSID", false /* reportMissing */)
+    property Fact _followOffsetX: controller.getParameterFact(-1, "FOLL_OFS_X", false /* reportMissing */)
+    property Fact _followOffsetY: controller.getParameterFact(-1, "FOLL_OFS_Y", false /* reportMissing */)
+    property Fact _followOffsetZ: controller.getParameterFact(-1, "FOLL_OFS_Z", false /* reportMissing */)
+    property Fact _followOffsetType: controller.getParameterFact(-1, "FOLL_OFS_TYPE", false /* reportMissing */)
+    property Fact _followAltitudeType: controller.getParameterFact(-1, "FOLL_ALT_TYPE", false /* reportMissing */)
+    property Fact _followYawBehavior: controller.getParameterFact(-1, "FOLL_YAW_BEHAVE", false /* reportMissing */)
 
     Column {
-        anchors.fill:       parent
+        anchors.fill: parent
 
         VehicleSummaryRow {
-            labelText: qsTr("Batt1 monitor")
-            valueText: _batt1Monitor.enumStringValue
+            labelText: qsTr("Follow Enabled")
+            valueText: _followEnabled.enumStringValue
         }
 
         VehicleSummaryRow {
-            labelText: qsTr("Batt1 capacity")
-            valueText:  _batt1MonitorEnabled ? _battCapacity.valueString + " " + _battCapacity.units : ""
-            visible:    _batt1MonitorEnabled
+            labelText: qsTr("Follow System ID")
+            valueText: _followSysId.valueString
+            visible: _followParamsAvailable
         }
 
         VehicleSummaryRow {
-            labelText:  qsTr("Batt2 monitor")
-            valueText:  _batt2MonitorAvailable ? _batt2Monitor.enumStringValue : ""
-            visible:    _batt2MonitorAvailable
+            labelText: qsTr("Follow Max Distance")
+            valueText: _followDistanceMax.valueString + " " + _followDistanceMax.units
+            visible: _followParamsAvailable
         }
 
         VehicleSummaryRow {
-            labelText:  qsTr("Batt2 capacity")
-            valueText:  _batt2MonitorEnabled ? _batt2Capacity.valueString + " " + _batt2Capacity.units : ""
-            visible:    _batt2MonitorEnabled
+            labelText: qsTr("Follow Offset X")
+            valueText: _followOffsetX.valueString + " " + _followOffsetX.units
+            visible: _followParamsAvailable
+        }
+
+        VehicleSummaryRow {
+            labelText: qsTr("Follow Offset Y")
+            valueText: _followOffsetY.valueString + " " + _followOffsetY.units
+            visible: _followParamsAvailable
+        }
+
+        VehicleSummaryRow {
+            labelText: qsTr("Follow Offset Z")
+            valueText: _followOffsetZ.valueString + " " + _followOffsetZ.units
+            visible: _followParamsAvailable
+        }
+
+        VehicleSummaryRow {
+            labelText: qsTr("Follow Offset Type")
+            valueText: _followOffsetType.enumStringValue
+            visible: _followParamsAvailable
+        }
+
+        VehicleSummaryRow {
+            labelText: qsTr("Follow Altitude Type")
+            valueText: _followAltitudeType.enumStringValue
+            visible: _followParamsAvailable
+        }
+
+        VehicleSummaryRow {
+            labelText: qsTr("Follow Yaw Behavior")
+            valueText: _followYawBehavior.enumStringValue
+            visible: _followParamsAvailable
         }
     }
 }
diff --git a/src/Comms/MAVLinkProtocol.cc b/src/Comms/MAVLinkProtocol.cc
index 5c1e6866a4d..fc9f84a4ae1 100644
--- a/src/Comms/MAVLinkProtocol.cc
+++ b/src/Comms/MAVLinkProtocol.cc
@@ -406,7 +406,7 @@ void MAVLinkProtocol::setSystemId(int id)
 }
 
 /** @return Component id of this application */
-int MAVLinkProtocol::getComponentId()
+int MAVLinkProtocol::getComponentId() const
 {
     return MAV_COMP_ID_MISSIONPLANNER;
 }
diff --git a/src/Comms/MAVLinkProtocol.h b/src/Comms/MAVLinkProtocol.h
index bb779a0581e..23e8bc79e9f 100644
--- a/src/Comms/MAVLinkProtocol.h
+++ b/src/Comms/MAVLinkProtocol.h
@@ -43,7 +43,7 @@ class MAVLinkProtocol : public QGCTool
     /** @brief Get the system id of this application */
     int getSystemId() const;
     /** @brief Get the component id of this application */
-    int getComponentId();
+    int getComponentId() const;
 
     /** @brief Get protocol version check state */
     bool versionCheckEnabled() const {
diff --git a/src/FactSystem/FactControls/FactPanelController.cc b/src/FactSystem/FactControls/FactPanelController.cc
index 61cc3508f08..c7e1c67d68e 100644
--- a/src/FactSystem/FactControls/FactPanelController.cc
+++ b/src/FactSystem/FactControls/FactPanelController.cc
@@ -22,7 +22,8 @@
 
 QGC_LOGGING_CATEGORY(FactPanelControllerLog, "FactPanelControllerLog")
 
-FactPanelController::FactPanelController()
+FactPanelController::FactPanelController(QObject *parent)
+    : QObject(parent)
 {
     _vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
     if (_vehicle) {
diff --git a/src/FactSystem/FactControls/FactPanelController.h b/src/FactSystem/FactControls/FactPanelController.h
index e9b342bdd00..5c72a1949d7 100644
--- a/src/FactSystem/FactControls/FactPanelController.h
+++ b/src/FactSystem/FactControls/FactPanelController.h
@@ -31,7 +31,7 @@ class FactPanelController : public QObject
     Q_MOC_INCLUDE("Vehicle.h")
     Q_MOC_INCLUDE("Fact.h")
 public:
-    FactPanelController();
+    FactPanelController(QObject *parent = nullptr);
 
     Q_PROPERTY(Vehicle* vehicle MEMBER _vehicle CONSTANT)
 
diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
index 20a133f1971..2ab084ca0f3 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
@@ -1074,13 +1074,15 @@ void APMFirmwarePlugin::_handleRCChannelsRaw(Vehicle* vehicle, mavlink_message_t
     }
 }
 
-void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities)
+void APMFirmwarePlugin::sendGCSMotionReport(Vehicle *vehicle, FollowMe::GCSMotionReport &motionReport, uint8_t estimationCapabilities)
 {
+    Q_CHECK_PTR(vehicle);
+
     if (!vehicle->homePosition().isValid()) {
         static bool sentOnce = false;
         if (!sentOnce) {
             sentOnce = true;
-            qgcApp()->showAppMessage(tr("Follow failed: Home position not set."));
+            qgcApp()->showAppMessage(QStringLiteral("Follow failed: Home position not set."));
         }
         return;
     }
@@ -1089,38 +1091,39 @@ void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMoti
         static bool sentOnce = false;
         if (!sentOnce) {
             sentOnce = true;
-            qWarning() << "APMFirmwarePlugin::_sendGCSMotionReport estimateCapabilities" << estimationCapabilities;
-            qgcApp()->showAppMessage(tr("Follow failed: Ground station cannot provide required position information."));
+            qCWarning(APMFirmwarePluginLog) << Q_FUNC_INFO << "estimateCapabilities" << estimationCapabilities;
+            qgcApp()->showAppMessage(QStringLiteral("Follow failed: Ground station cannot provide required position information."));
         }
         return;
     }
 
     SharedLinkInterfacePtr sharedLink = vehicle->vehicleLinkManager()->primaryLink().lock();
-    if (sharedLink) {
-        MAVLinkProtocol*                mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
-        mavlink_global_position_int_t   globalPositionInt;
-
-        memset(&globalPositionInt, 0, sizeof(globalPositionInt));
-
-        // Important note: QGC only supports sending the constant GCS home position altitude for follow me.
-        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>(vehicle->homePosition().altitude() * 1000);    // mm
-        globalPositionInt.relative_alt =    static_cast<int32_t>(0);                                            // 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 =             static_cast<uint16_t>(motionReport.headingDegrees * 100.0);         // centi-degrees
-
-        mavlink_message_t message;
-        mavlink_msg_global_position_int_encode_chan(static_cast<uint8_t>(mavlinkProtocol->getSystemId()),
-                                                    static_cast<uint8_t>(mavlinkProtocol->getComponentId()),
-                                                    sharedLink->mavlinkChannel(),
-                                                    &message,
-                                                    &globalPositionInt);
-        vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message);
+    if (!sharedLink) {
+        return;
     }
+
+    const MAVLinkProtocol* const mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
+    const mavlink_global_position_int_t globalPositionInt = {
+        static_cast<uint32_t>(qgcApp()->msecsSinceBoot()),                  /*< [ms] Timestamp (time since system boot).*/
+        motionReport.lat_int,                                               /*< [degE7] Latitude, expressed*/
+        motionReport.lon_int,                                               /*< [degE7] Longitude, expressed*/
+        static_cast<int32_t>(vehicle->homePosition().altitude() * 1000),    /*< [mm] Altitude (MSL).*/
+        static_cast<int32_t>(0),                                            /*< [mm] Altitude above home*/
+        static_cast<int16_t>(motionReport.vxMetersPerSec * 100),            /*< [cm/s] Ground X Speed (Latitude, positive north)*/
+        static_cast<int16_t>(motionReport.vyMetersPerSec * 100),            /*< [cm/s] Ground Y Speed (Longitude, positive east)*/
+        static_cast<int16_t>(motionReport.vzMetersPerSec * 100),            /*< [cm/s] Ground Z Speed (Altitude, positive down)*/
+        static_cast<uint16_t>(motionReport.headingDegrees * 100.0)          /*< [cdeg] Vehicle heading (yaw angle)*/
+    };
+
+    mavlink_message_t message;
+    (void) mavlink_msg_global_position_int_encode_chan(
+        static_cast<uint8_t>(mavlinkProtocol->getSystemId()),
+        static_cast<uint8_t>(mavlinkProtocol->getComponentId()),
+        sharedLink->mavlinkChannel(),
+        &message,
+        &globalPositionInt
+    );
+    vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message);
 }
 
 uint8_t APMFirmwarePlugin::_reencodeMavlinkChannel()
diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
index 9dd53790b64..61869e5c97e 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
@@ -86,6 +86,7 @@ class APMFirmwarePlugin : public FirmwarePlugin
     bool                fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle) override;
     void                guidedModeChangeEquivalentAirspeedMetersSecond(Vehicle* vehicle, double airspeed_equiv) override;
     QVariant            mainStatusIndicatorContentItem  (const Vehicle* vehicle) const override;
+    void                sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) override;
 
     // support for changing speed in Copter guide mode:
     bool mulirotorSpeedLimitsAvailable(Vehicle* vehicle) override;
@@ -102,7 +103,6 @@ class APMFirmwarePlugin : public FirmwarePlugin
     APMFirmwarePlugin(void);
 
     void setSupportedModes  (QList<APMCustomMode> supportedModes);
-    void _sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities);
 
     static void _setBaroGndTemp(Vehicle* vehicle, qreal temperature);
     static void _setBaroAltOffset(Vehicle* vehicle, qreal offset);
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
index 04cc553adde..bd646f104d2 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
@@ -134,8 +134,3 @@ bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle)
 {
     return vehicle->parameterManager()->getParameter(ParameterManager::defaultComponentId, "FRAME")->rawValue().toInt() != 0;
 }
-
-void ArduCopterFirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities)
-{
-    _sendGCSMotionReport(vehicle, motionReport, estimatationCapabilities);
-}
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
index 982909397a2..fbae38d3fb2 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
@@ -73,7 +73,6 @@ class ArduCopterFirmwarePlugin : public APMFirmwarePlugin
     QString followFlightMode                    (void) const override { return QStringLiteral("Follow"); }
     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;
diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc
index 3f8a249e025..6b2e91ad69d 100644
--- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc
@@ -81,9 +81,3 @@ bool ArduRoverFirmwarePlugin::supportsNegativeThrust(Vehicle* /*vehicle*/)
 {
     return true;
 }
-
-void ArduRoverFirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities)
-{
-    _sendGCSMotionReport(vehicle, motionReport, estimatationCapabilities);
-}
-
diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h
index 796900f8712..783c572d5b5 100644
--- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h
@@ -55,7 +55,6 @@ class ArduRoverFirmwarePlugin : public APMFirmwarePlugin
     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;
diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc
index 2d6d4aa7a1c..39a7514a999 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/FirmwarePlugin.cc
@@ -30,8 +30,6 @@ const QString guided_mode_not_supported_by_vehicle = QObject::tr("Guided mode no
 
 QVariantList FirmwarePlugin::_cameraList;
 
-const QString FirmwarePlugin::px4FollowMeFlightMode(QObject::tr("Follow Me"));
-
 FirmwarePlugin::FirmwarePlugin(void)
 {
     qmlRegisterType<RadioComponentController>       ("QGroundControl.Controllers",                       1, 0, "RadioComponentController");
@@ -1112,31 +1110,37 @@ QString FirmwarePlugin::gotoFlightMode(void) const
     return QString();
 }
 
-void FirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities)
+void FirmwarePlugin::sendGCSMotionReport(Vehicle *vehicle, FollowMe::GCSMotionReport &motionReport, uint8_t estimationCapabilities)
 {
+    Q_CHECK_PTR(vehicle);
+
     SharedLinkInterfacePtr sharedLink = vehicle->vehicleLinkManager()->primaryLink().lock();
-    if (sharedLink) {
-        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()),
-                                              sharedLink->mavlinkChannel(),
-                                              &message,
-                                              &follow_target);
-        vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message);
+    if (!sharedLink) {
+        return;
     }
+
+    const MAVLinkProtocol* const mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
+    mavlink_follow_target_t follow_target{0};
+
+    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()),
+        sharedLink->mavlinkChannel(),
+        &message,
+        &follow_target
+    );
+    vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message);
 }
 
 Autotune* FirmwarePlugin::createAutotune(Vehicle *vehicle)
diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h
index c1573d346b1..aed823e4c35 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.h
+++ b/src/FirmwarePlugin/FirmwarePlugin.h
@@ -349,10 +349,7 @@ class FirmwarePlugin : public QObject
     virtual void adjustMetaData(MAV_TYPE /*vehicleType*/, FactMetaData* /*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;
+    virtual void sendGCSMotionReport(Vehicle *vehicle, FollowMe::GCSMotionReport &motionReport, uint8_t estimationCapabilities);
 
     // gets hobbs meter from autopilot. This should be reimplmeented for each firmware
     virtual QString getHobbsMeter(Vehicle* vehicle) { Q_UNUSED(vehicle); return "Not Supported"; }
diff --git a/src/FollowMe/CMakeLists.txt b/src/FollowMe/CMakeLists.txt
index e7f019ca96d..e705fd61df3 100644
--- a/src/FollowMe/CMakeLists.txt
+++ b/src/FollowMe/CMakeLists.txt
@@ -10,12 +10,12 @@ target_link_libraries(FollowMe
         Qt6::Positioning
         FirmwarePlugin
         PositionManager
+        QGC
         Settings
         Utilities
         Vehicle
     PUBLIC
         Qt6::Core
-        QGC
 )
 
 target_include_directories(FollowMe PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
diff --git a/src/FollowMe/FollowMe.cc b/src/FollowMe/FollowMe.cc
index ee87de49bde..3a13cfc0782 100644
--- a/src/FollowMe/FollowMe.cc
+++ b/src/FollowMe/FollowMe.cc
@@ -8,6 +8,7 @@
  ****************************************************************************/
 
 #include "FollowMe.h"
+#include "QGCApplication.h"
 #include "MultiVehicleManager.h"
 #include "FirmwarePlugin.h"
 #include "Vehicle.h"
@@ -18,42 +19,59 @@
 
 #include <QtPositioning/QGeoPositionInfo>
 
-QGC_LOGGING_CATEGORY(FollowMeLog, "FollowMeLog")
+QGC_LOGGING_CATEGORY(FollowMeLog, "qgc.followme")
 
-FollowMe::FollowMe(QGCApplication* app, QGCToolbox* toolbox)
-    : QGCTool(app, toolbox)
+Q_APPLICATION_STATIC(FollowMe, _followMeInstance);
+
+FollowMe::FollowMe(QObject *parent)
+    : QObject(parent)
+    , _gcsMotionReportTimer(new QTimer(this))
+{
+    // qCDebug(FollowMeLog) << Q_FUNC_INFO << this;
+
+    _gcsMotionReportTimer->setSingleShot(false);
+}
+
+FollowMe::~FollowMe()
 {
-    _gcsMotionReportTimer.setSingleShot(false);
+    // qCDebug(FollowMeLog) << Q_FUNC_INFO << this;
 }
 
-void FollowMe::setToolbox(QGCToolbox* toolbox)
+FollowMe *FollowMe::instance()
 {
-    QGCTool::setToolbox(toolbox);
+    return _followMeInstance();
+}
 
-    connect(&_gcsMotionReportTimer,                                     &QTimer::timeout,       this, &FollowMe::_sendGCSMotionReport);
-    connect(toolbox->settingsManager()->appSettings()->followTarget(),  &Fact::rawValueChanged, this, &FollowMe::_settingsChanged);
+void FollowMe::init()
+{
+    static bool once = false;
+    if (!once) {
+        (void) connect(_gcsMotionReportTimer, &QTimer::timeout, this, &FollowMe::_sendGCSMotionReport);
+        (void) connect(qgcApp()->toolbox()->settingsManager()->appSettings()->followTarget(), &Fact::rawValueChanged, this, &FollowMe::_settingsChanged);
 
-    _settingsChanged();
+        _settingsChanged(qgcApp()->toolbox()->settingsManager()->appSettings()->followTarget()->rawValue());
+    }
+    once = true;
 }
 
-void FollowMe::_settingsChanged()
+void FollowMe::_settingsChanged(QVariant value)
 {
-    _currentMode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt();
+    _currentMode = static_cast<FollowMode>(value.toUInt());
 
     switch (_currentMode) {
     case MODE_NEVER:
-        disconnect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded,     this, &FollowMe::_vehicleAdded);
-        disconnect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved,   this, &FollowMe::_vehicleRemoved);
+        (void) disconnect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &FollowMe::_vehicleAdded);
+        (void) disconnect(qgcApp()->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);
+        (void) connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &FollowMe::_vehicleAdded);
+        (void) connect(qgcApp()->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);
+        (void) connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &FollowMe::_vehicleAdded);
+        (void) connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved, this, &FollowMe::_vehicleRemoved);
         _enableIfVehicleInFollow();
         break;
     }
@@ -61,23 +79,23 @@ void FollowMe::_settingsChanged()
 
 void FollowMe::_enableFollowSend()
 {
-    if (!_gcsMotionReportTimer.isActive()) {
-        _gcsMotionReportTimer.setInterval(qMin(_toolbox->qgcPositionManager()->updateInterval(), 250));
-        _gcsMotionReportTimer.start();
+    if (!_gcsMotionReportTimer->isActive()) {
+        _gcsMotionReportTimer->setInterval(qMin(qgcApp()->toolbox()->qgcPositionManager()->updateInterval(), kMotionUpdateInterval));
+        _gcsMotionReportTimer->start();
     }
 }
 
 void FollowMe::_disableFollowSend()
 {
-    if (_gcsMotionReportTimer.isActive()) {
-        _gcsMotionReportTimer.stop();
+    if (_gcsMotionReportTimer->isActive()) {
+        _gcsMotionReportTimer->stop();
     }
 }
 
 void FollowMe::_sendGCSMotionReport()
 {
-    QGeoPositionInfo    geoPositionInfo =   _toolbox->qgcPositionManager()->geoPositionInfo();
-    QGeoCoordinate      gcsCoordinate =     geoPositionInfo.coordinate();
+    const QGeoPositionInfo geoPositionInfo = qgcApp()->toolbox()->qgcPositionManager()->geoPositionInfo();
+    const QGeoCoordinate gcsCoordinate = geoPositionInfo.coordinate();
 
     if (!geoPositionInfo.isValid()) {
         return;
@@ -88,9 +106,9 @@ void FollowMe::_sendGCSMotionReport()
     if (_currentMode == MODE_ALWAYS) {
         needFollowMe = true;
     } else if (_currentMode == MODE_FOLLOWME) {
-        QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles();
-        for (int i=0; i<vehicles->count(); i++) {
-            Vehicle* vehicle = vehicles->value<Vehicle*>(i);
+        QmlObjectListModel* const vehicles = qgcApp()->toolbox()->multiVehicleManager()->vehicles();
+        for (int i = 0; i < vehicles->count(); i++) {
+            const Vehicle* const vehicle = vehicles->value<const Vehicle*>(i);
             if (_isFollowFlightMode(vehicle, vehicle->flightMode())) {
                 needFollowMe = true;
             }
@@ -100,92 +118,84 @@ void FollowMe::_sendGCSMotionReport()
         return;
     }
 
-    GCSMotionReport motionReport = {};
-    uint8_t         estimatation_capabilities = 0;
+    GCSMotionReport motionReport{0};
+    uint8_t estimationCapabilities = 0;
 
-    // get the current location coordinates
+    // Get the current location coordinates
+    // Important note: QGC only supports sending the constant GCS home position altitude for follow me.
+    motionReport.lat_int = static_cast<int>(gcsCoordinate.latitude() * 1e7);
+    motionReport.lon_int = static_cast<int>(gcsCoordinate.longitude() * 1e7);
+    motionReport.altMetersAMSL = gcsCoordinate.altitude();
+    estimationCapabilities |= (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);
-
-    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) == true) {
-        estimatation_capabilities |= (1 << HEADING);
+    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction)) {
+        estimationCapabilities |= (1 << HEADING);
         motionReport.headingDegrees = geoPositionInfo.attribute(QGeoPositionInfo::Direction);
     }
 
-    // get the current eph and epv
-
+    // get the current eph
     if (geoPositionInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) {
         motionReport.pos_std_dev[0] = motionReport.pos_std_dev[1] = geoPositionInfo.attribute(QGeoPositionInfo::HorizontalAccuracy);
     }
 
+    // get the current epv
     if (geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) {
         motionReport.pos_std_dev[2] = geoPositionInfo.attribute(QGeoPositionInfo::VerticalAccuracy);
     }
 
     // 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
+    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) && geoPositionInfo.hasAttribute(QGeoPositionInfo::GroundSpeed)) {
+        estimationCapabilities |= (1 << VEL);
 
-    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) && geoPositionInfo.hasAttribute(QGeoPositionInfo::GroundSpeed) == true) {
-        estimatation_capabilities |= (1 << VEL);
-
-        qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction));
-        qreal velocity  = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
+        const qreal direction = qDegreesToRadians(geoPositionInfo.attribute(QGeoPositionInfo::Direction));
+        const qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
 
-        motionReport.vxMetersPerSec = cos(direction)*velocity;
-        motionReport.vyMetersPerSec = sin(direction)*velocity;
+        motionReport.vxMetersPerSec = cos(direction) * velocity;
+        motionReport.vyMetersPerSec = sin(direction) * velocity;
     } else {
         motionReport.vxMetersPerSec = 0;
         motionReport.vyMetersPerSec = 0;
     }
 
-    QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles();
+    QmlObjectListModel* const vehicles = qgcApp()->toolbox()->multiVehicleManager()->vehicles();
 
-    for (int i=0; i<vehicles->count(); i++) {
-        Vehicle* vehicle = vehicles->value<Vehicle*>(i);
-        if (_currentMode == MODE_ALWAYS || _isFollowFlightMode(vehicle, vehicle->flightMode())) {
+    for (int i = 0; i < vehicles->count(); i++) {
+        Vehicle* const vehicle = vehicles->value<Vehicle*>(i);
+        if ((_currentMode == MODE_ALWAYS) || (_isFollowFlightMode(vehicle, vehicle->flightMode()))) {
             qCDebug(FollowMeLog) << "sendGCSMotionReport latInt:lonInt:altMetersAMSL" << motionReport.lat_int << motionReport.lon_int << motionReport.altMetersAMSL;
-            vehicle->firmwarePlugin()->sendGCSMotionReport(vehicle, motionReport, estimatation_capabilities);
+            vehicle->firmwarePlugin()->sendGCSMotionReport(vehicle, motionReport, estimationCapabilities);
         }
     }
 }
 
-double FollowMe::_degreesToRadian(double deg)
-{
-    return deg * M_PI / 180.0;
-}
-
-void FollowMe::_vehicleAdded(Vehicle* vehicle)
+void FollowMe::_vehicleAdded(Vehicle *vehicle)
 {
-    connect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow);
+    (void) connect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow);
     _enableIfVehicleInFollow();
 }
 
-void FollowMe::_vehicleRemoved(Vehicle* vehicle)
+void FollowMe::_vehicleRemoved(Vehicle *vehicle)
 {
-    disconnect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow);
+    (void) disconnect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow);
     _enableIfVehicleInFollow();
 }
 
-void FollowMe::_enableIfVehicleInFollow(void)
+void FollowMe::_enableIfVehicleInFollow()
 {
     if (_currentMode == MODE_ALWAYS) {
-        // System always enabled
         return;
     }
 
     // Any vehicle in follow mode will enable the system
-    QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles();
+    QmlObjectListModel* const vehicles = qgcApp()->toolbox()->multiVehicleManager()->vehicles();
 
-    for (int i=0; i< vehicles->count(); i++) {
-        Vehicle* vehicle = vehicles->value<Vehicle*>(i);
+    for (int i = 0; i < vehicles->count(); i++) {
+        const Vehicle* const vehicle = vehicles->value<const Vehicle*>(i);
         if (_isFollowFlightMode(vehicle, vehicle->flightMode())) {
             _enableFollowSend();
             return;
@@ -195,7 +205,7 @@ void FollowMe::_enableIfVehicleInFollow(void)
     _disableFollowSend();
 }
 
-bool FollowMe::_isFollowFlightMode (Vehicle* vehicle, const QString& flightMode)
+bool FollowMe::_isFollowFlightMode(const Vehicle *vehicle, const QString &flightMode)
 {
-    return flightMode.compare(vehicle->followFlightMode()) == 0;
+    return (flightMode.compare(vehicle->followFlightMode()) == 0);
 }
diff --git a/src/FollowMe/FollowMe.h b/src/FollowMe/FollowMe.h
index c8e11451681..ae55cf205f8 100644
--- a/src/FollowMe/FollowMe.h
+++ b/src/FollowMe/FollowMe.h
@@ -10,35 +10,40 @@
 
 #pragma once
 
-#include <QtCore/QTimer>
 #include <QtCore/QLoggingCategory>
-
-#include "QGCToolbox.h"
+#include <QtCore/QObject>
+#include <QtCore/QTimer>
+#include <QtCore/QVariant>
 
 Q_DECLARE_LOGGING_CATEGORY(FollowMeLog)
 
 class Vehicle;
 
-class FollowMe : public QGCTool
+class FollowMe : public QObject
 {
     Q_OBJECT
+    Q_MOC_INCLUDE("Vehicle.h")
 
 public:
-    FollowMe(QGCApplication* app, QGCToolbox* toolbox);
+    explicit FollowMe(QObject *parent = nullptr);
+    ~FollowMe();
+
+    static FollowMe* instance();
+    void init();
 
     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
+        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 {
+    /// Mavlink defined motion reporting capabilities
+    enum MotionCapability {
         POS = 0,
         VEL = 1,
         ACCEL = 2,
@@ -46,27 +51,26 @@ class FollowMe : public QGCTool
         HEADING = 4
     };
 
-    void setToolbox(QGCToolbox* toolbox) override;
-
 private slots:
-    void _sendGCSMotionReport       (void);
-    void _settingsChanged           (void);
-    void _vehicleAdded              (Vehicle* vehicle);
-    void _vehicleRemoved            (Vehicle* vehicle);
-    void _enableIfVehicleInFollow   (void);
+    void _sendGCSMotionReport();
+    void _settingsChanged(QVariant value);
+    void _vehicleAdded(Vehicle *vehicle);
+    void _vehicleRemoved(Vehicle *vehicle);
+    void _enableIfVehicleInFollow();
 
 private:
-    enum {
+    enum FollowMode {
         MODE_NEVER,
         MODE_ALWAYS,
         MODE_FOLLOWME
     };
 
-    void    _disableFollowSend  (void);
-    void    _enableFollowSend   (void);
-    double  _degreesToRadian    (double deg);
-    bool    _isFollowFlightMode (Vehicle* vehicle, const QString& flightMode);
+    void _disableFollowSend();
+    void _enableFollowSend();
+    bool _isFollowFlightMode(const Vehicle *vehicle, const QString &flightMode);
+
+    QTimer *_gcsMotionReportTimer = nullptr;
+    FollowMode _currentMode = MODE_NEVER;
 
-    QTimer      _gcsMotionReportTimer;
-    uint32_t    _currentMode;
+    static constexpr int kMotionUpdateInterval = 250;
 };
diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc
index de83b386231..7c21bd4e5f1 100644
--- a/src/QGCApplication.cc
+++ b/src/QGCApplication.cc
@@ -95,6 +95,7 @@
 #include "CustomAction.h"
 #include "CustomActionManager.h"
 #include "AudioOutput.h"
+#include "FollowMe.h"
 #include "JsonHelper.h"
 // #ifdef QGC_VIEWER3D
 #include "Viewer3DManager.h"
@@ -421,6 +422,8 @@ void QGCApplication::_initForNormalAppBoot()
     });
     AudioOutput::instance()->setMuted( _toolbox->settingsManager()->appSettings()->audioMuted()->rawValue().toBool() );
 
+    FollowMe::instance()->init();
+
     // Image provider for PX4 Flow
     _qmlAppEngine->addImageProvider(qgcImageProviderId, new QGCImageProvider());
 
diff --git a/src/QGCToolbox.cc b/src/QGCToolbox.cc
index 4cb9177e5cd..9fc0d696913 100644
--- a/src/QGCToolbox.cc
+++ b/src/QGCToolbox.cc
@@ -17,7 +17,6 @@
 #include "MAVLinkProtocol.h"
 #include "MissionCommandTree.h"
 #include "MultiVehicleManager.h"
-#include "FollowMe.h"
 #include "PositionManager.h"
 #include "VideoManager.h"
 #include "MAVLinkLogManager.h"
@@ -55,7 +54,6 @@ QGCToolbox::QGCToolbox(QGCApplication* app)
     _missionCommandTree     = new MissionCommandTree        (app, this);
     _multiVehicleManager    = new MultiVehicleManager       (app, this);
     _qgcPositionManager     = new QGCPositionManager        (app, this);
-    _followMe               = new FollowMe                  (app, this);
     _videoManager           = new VideoManager              (app, this);
 
     _mavlinkLogManager      = new MAVLinkLogManager         (app, this);
@@ -83,7 +81,6 @@ void QGCToolbox::setChildToolboxes(void)
     _mavlinkProtocol->setToolbox(this);
     _missionCommandTree->setToolbox(this);
     _multiVehicleManager->setToolbox(this);
-    _followMe->setToolbox(this);
     _qgcPositionManager->setToolbox(this);
     _videoManager->setToolbox(this);
     _mavlinkLogManager->setToolbox(this);
diff --git a/src/QGCToolbox.h b/src/QGCToolbox.h
index d5390cabbec..687873911f0 100644
--- a/src/QGCToolbox.h
+++ b/src/QGCToolbox.h
@@ -16,7 +16,6 @@
 class FirmwarePluginManager;
 class GPSManager;
 class JoystickManager;
-class FollowMe;
 class LinkManager;
 class MAVLinkProtocol;
 class MissionCommandTree;
@@ -48,7 +47,6 @@ class QGCToolbox : public QObject {
     MAVLinkProtocol*            mavlinkProtocol         () { return _mavlinkProtocol; }
     MissionCommandTree*         missionCommandTree      () { return _missionCommandTree; }
     MultiVehicleManager*        multiVehicleManager     () { return _multiVehicleManager; }
-    FollowMe*                   followMe                () { return _followMe; }
     QGCPositionManager*         qgcPositionManager      () { return _qgcPositionManager; }
     VideoManager*               videoManager            () { return _videoManager; }
     MAVLinkLogManager*          mavlinkLogManager       () { return _mavlinkLogManager; }
@@ -78,7 +76,6 @@ class QGCToolbox : public QObject {
     MAVLinkProtocol*            _mavlinkProtocol        = nullptr;
     MissionCommandTree*         _missionCommandTree     = nullptr;
     MultiVehicleManager*        _multiVehicleManager    = nullptr;
-    FollowMe*                   _followMe               = nullptr;
     QGCPositionManager*         _qgcPositionManager     = nullptr;
     VideoManager*               _videoManager           = nullptr;
     MAVLinkLogManager*          _mavlinkLogManager      = nullptr;
diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h
index ab03724d52d..110593fad0b 100644
--- a/src/Vehicle/MultiVehicleManager.h
+++ b/src/Vehicle/MultiVehicleManager.h
@@ -21,7 +21,6 @@
 #include "QmlObjectListModel.h"
 
 class FirmwarePluginManager;
-class FollowMe;
 class JoystickManager;
 class QGCApplication;
 class MAVLinkProtocol;
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 898d1acc5cf..82b23f16d40 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -44,6 +44,9 @@ add_qgc_test(FactSystemTestGeneric)
 add_qgc_test(FactSystemTestPX4)
 add_qgc_test(ParameterManagerTest)
 
+add_subdirectory(FollowMe)
+add_qgc_test(FollowMeTest)
+
 add_subdirectory(Geo)
 add_qgc_test(GeoTest)
 
@@ -122,6 +125,7 @@ target_link_libraries(qgctest
         CommsTest
         CompressionTest
         FactSystemTest
+        FollowMeTest
         GeoTest
         MAVLinkTest
         MissionManagerTest
diff --git a/test/FollowMe/CMakeLists.txt b/test/FollowMe/CMakeLists.txt
new file mode 100644
index 00000000000..1866931a428
--- /dev/null
+++ b/test/FollowMe/CMakeLists.txt
@@ -0,0 +1,19 @@
+find_package(Qt6 REQUIRED COMPONENTS Core Test)
+
+qt_add_library(FollowMeTest
+    STATIC
+        FollowMeTest.cc
+        FollowMeTest.h
+)
+
+target_link_libraries(FollowMeTest
+    PRIVATE
+        Qt6::Test
+        FollowMe
+        QGC
+        Vehicle
+    PUBLIC
+        qgcunittest
+)
+
+target_include_directories(FollowMeTest PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
diff --git a/test/FollowMe/FollowMeTest.cc b/test/FollowMe/FollowMeTest.cc
new file mode 100644
index 00000000000..d6d71ca856b
--- /dev/null
+++ b/test/FollowMe/FollowMeTest.cc
@@ -0,0 +1,35 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "FollowMeTest.h"
+#include "FollowMe.h"
+#include "QGCApplication.h"
+#include "MultiVehicleManager.h"
+#include "Vehicle.h"
+#include "SettingsManager.h"
+
+#include <QtTest/QTest>
+#include <QtTest/QSignalSpy>
+
+void FollowMeTest::_testFollowMe()
+{
+    _connectMockLinkNoInitialConnectSequence();
+
+    MultiVehicleManager *vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle *vehicle = vehicleMgr->activeVehicle();
+    vehicle->setFlightMode(vehicle->followFlightMode());
+    FollowMe::instance()->init();
+    qgcApp()->toolbox()->settingsManager()->appSettings()->followTarget()->setRawValue(1);
+
+    QSignalSpy spyGCSMotionReport(vehicle, &Vehicle::messagesSentChanged);
+
+    QVERIFY(spyGCSMotionReport.wait(1000));
+
+    _disconnectMockLink();
+}
diff --git a/test/FollowMe/FollowMeTest.h b/test/FollowMe/FollowMeTest.h
new file mode 100644
index 00000000000..a426f1c52fe
--- /dev/null
+++ b/test/FollowMe/FollowMeTest.h
@@ -0,0 +1,20 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "UnitTest.h"
+
+class FollowMeTest : public UnitTest
+{
+    Q_OBJECT
+
+private slots:
+    void _testFollowMe();
+};
diff --git a/test/UnitTestList.cc b/test/UnitTestList.cc
index 44cb765bd6b..7765f293540 100644
--- a/test/UnitTestList.cc
+++ b/test/UnitTestList.cc
@@ -32,6 +32,9 @@
 #include "FactSystemTestPX4.h"
 #include "ParameterManagerTest.h"
 
+// FollowMe
+#include "FollowMeTest.h"
+
 // Geo
 #include "GeoTest.h"
 
@@ -121,6 +124,9 @@ int runTests(bool stress, QStringView unitTestOptions)
 	UT_REGISTER_TEST(FactSystemTestPX4)
 	UT_REGISTER_TEST(ParameterManagerTest)
 
+	// FollowMe
+	UT_REGISTER_TEST(FollowMeTest)
+
 	// Geo
     UT_REGISTER_TEST(GeoTest)