FollowMe: Re-enable for Ardupilot

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
 

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:

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
 }
 ]
 }

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

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 { 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;
 };

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()

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";
 };

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

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 {

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) {

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)
 

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -1074,13 +1074,16 @@ void APMFirmwarePlugin::_handleRCChannelsRaw(Vehicle* vehicle, mavlink_message_t
     }
 }
 
-void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities)
+/// Important note: QGC only supports sending the constant GCS home position altitude for follow me.
+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 +1092,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()