Make Follow Target stream a setting.

src/FollowMe/FollowMe.cc

@@ -16,25 +16,37 @@
 #include "FollowMe.h"
 #include "Vehicle.h"
 #include "PositionManager.h"
+#include "SettingsManager.h"
+#include "AppSettings.h"
 
 FollowMe::FollowMe(QGCApplication* app, QGCToolbox* toolbox)
     : QGCTool(app, toolbox), estimatation_capabilities(0)
-    , _manualControl(false)
 {
     memset(&_motionReport, 0, sizeof(motionReport_s));
     runTime.start();
     _gcsMotionReportTimer.setSingleShot(false);
+}
+
+void FollowMe::setToolbox(QGCToolbox* toolbox)
+{
+    QGCTool::setToolbox(toolbox);
     connect(&_gcsMotionReportTimer, &QTimer::timeout, this, &FollowMe::_sendGCSMotionReport);
+    connect(toolbox->settingsManager()->appSettings()->followTarget(), &Fact::rawValueChanged, this, &FollowMe::_settingsChanged);
+    _currentMode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt();
+    if(_currentMode == MODE_ALWAYS) {
+        _enable();
+    }
 }
 
 void FollowMe::followMeHandleManager(const QString&)
 {
-    if(!_manualControl) {
+    //-- If we are to change based on current flight mode
+    if(_currentMode == MODE_FOLLOWME) {
         QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles();
         for (int i=0; i< vehicles.count(); i++) {
             Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
             if (vehicle->px4Firmware() && vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
-                _enable(_toolbox->qgcPositionManager()->updateInterval());
+                _enable();
                 return;
             }
         }
@@ -42,25 +54,40 @@ void FollowMe::followMeHandleManager(const QString&)
     }
 }
 
-void FollowMe::manualEnable(int interval)
+void FollowMe::_settingsChanged()
 {
-    _manualControl = true;
-    _enable(interval);
-}
-
-void FollowMe::manualDisable()
-{
-    _manualControl = false;
-    _disable();
+    uint32_t mode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt();
+    if(_currentMode != mode) {
+        _currentMode = mode;
+        switch (mode) {
+        case MODE_NEVER:
+            if(_gcsMotionReportTimer.isActive()) {
+                _disable();
+            }
+            break;
+        case MODE_ALWAYS:
+            if(!_gcsMotionReportTimer.isActive()) {
+                _enable();
+            }
+            break;
+        case MODE_FOLLOWME:
+            if(!_gcsMotionReportTimer.isActive()) {
+                followMeHandleManager(QString());
+            }
+            break;
+        default:
+            break;
+        }
+    }
 }
 
-void FollowMe::_enable(int interval)
+void FollowMe::_enable()
 {
     connect(_toolbox->qgcPositionManager(),
             SIGNAL(positionInfoUpdated(QGeoPositionInfo)),
             this,
             SLOT(_setGPSLocation(QGeoPositionInfo)));
-    _gcsMotionReportTimer.setInterval(interval);
+    _gcsMotionReportTimer.setInterval(_toolbox->qgcPositionManager()->updateInterval());
     _gcsMotionReportTimer.start();
 }
 
@@ -75,15 +102,17 @@ void FollowMe::_disable()
 
 void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo)
 {
-    if (geoPositionInfo.isValid())
-    {
+    if (!geoPositionInfo.isValid()) {
+        //-- Invalidate cached coordinates
+        memset(&_motionReport, 0, sizeof(motionReport_s));
+    } else {
         // get the current location coordinates
 
         QGeoCoordinate geoCoordinate = geoPositionInfo.coordinate();
 
-        _motionReport.lat_int = geoCoordinate.latitude()*1e7;
-        _motionReport.lon_int = geoCoordinate.longitude()*1e7;
-        _motionReport.alt = geoCoordinate.altitude();
+        _motionReport.lat_int = geoCoordinate.latitude()  * 1e7;
+        _motionReport.lon_int = geoCoordinate.longitude() * 1e7;
+        _motionReport.alt     = geoCoordinate.altitude();
 
         estimatation_capabilities |= (1 << POS);
 
@@ -111,7 +140,7 @@ void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo)
             estimatation_capabilities |= (1 << VEL);
 
             qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction));
-            qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
+            qreal velocity  = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
 
             _motionReport.vx = cos(direction)*velocity;
             _motionReport.vy = sin(direction)*velocity;
@@ -123,15 +152,20 @@ void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo)
     }
 }
 
-void FollowMe::_sendGCSMotionReport(void)
+void FollowMe::_sendGCSMotionReport()
 {
-    QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles();
+
+    //-- Do we have any real data?
+    if(_motionReport.lat_int == 0 && _motionReport.lon_int == 0 && _motionReport.alt == 0) {
+        return;
+    }
+
+    QmlObjectListModel & vehicles    = *_toolbox->multiVehicleManager()->vehicles();
     MAVLinkProtocol* mavlinkProtocol = _toolbox->mavlinkProtocol();
     mavlink_follow_target_t follow_target;
-
     memset(&follow_target, 0, sizeof(follow_target));
 
-    follow_target.timestamp = runTime.nsecsElapsed()*1e-6;
+    follow_target.timestamp = runTime.nsecsElapsed() * 1e-6;
     follow_target.est_capabilities = estimatation_capabilities;
     follow_target.position_cov[0] = _motionReport.pos_std_dev[0];
     follow_target.position_cov[2] = _motionReport.pos_std_dev[2];
@@ -143,7 +177,7 @@ void FollowMe::_sendGCSMotionReport(void)
 
     for (int i=0; i< vehicles.count(); i++) {
         Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
-        if(_manualControl || vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
+        if(_currentMode || vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
             mavlink_message_t message;
             mavlink_msg_follow_target_encode_chan(mavlinkProtocol->getSystemId(),
                                                   mavlinkProtocol->getComponentId(),

src/FollowMe/FollowMe.h

@@ -29,15 +29,15 @@ class FollowMe : public QGCTool
 public:
     FollowMe(QGCApplication* app, QGCToolbox* toolbox);
 
-    void    manualEnable    (int interval);
-    void    manualDisable   ();
+    void setToolbox(QGCToolbox* toolbox) override;
 
 public slots:
-    void followMeHandleManager(const QString&);
+    void followMeHandleManager  (const QString&);
 
 private slots:
-    void _setGPSLocation(QGeoPositionInfo geoPositionInfo);
-    void _sendGCSMotionReport(void);
+    void _setGPSLocation        (QGeoPositionInfo geoPositionInfo);
+    void _sendGCSMotionReport   ();
+    void _settingsChanged       ();
 
 private:
     QElapsedTimer runTime;    
@@ -69,9 +69,15 @@ private:
     uint8_t estimatation_capabilities;
 
     void    _disable    ();
-    void    _enable     (int interval);
+    void    _enable     ();
 
     double  _degreesToRadian(double deg);
 
-    bool    _manualControl;
+    enum {
+        MODE_NEVER,
+        MODE_ALWAYS,
+        MODE_FOLLOWME
+    };
+
+    uint32_t _currentMode;
 };

src/Settings/App.SettingsGroup.json

@@ -183,5 +183,13 @@
     "shortDescription": "Default firmware type for flashing",
     "type":             "uint32",
     "defaultValue":     12
+},
+{
+    "name":             "FollowTarget",
+    "shortDescription": "Stream GCS' coordinates to Autopilot",
+    "type":             "uint32",
+    "enumStrings":      "Never,Always,When in Follow Me Flight Mode",
+    "enumValues":       "0,1,2",
+    "defaultValue":     0
 }
 ]

src/Settings/AppSettings.cc

@@ -37,6 +37,7 @@ const char* AppSettings::mapboxTokenName =                              "MapboxT
 const char* AppSettings::esriTokenName =                                "EsriToken";
 const char* AppSettings::defaultFirmwareTypeName =                      "DefaultFirmwareType";
 const char* AppSettings::gstDebugName =                                 "GstreamerDebugLevel";
+const char* AppSettings::followTargetName =                             "FollowTarget";
 
 const char* AppSettings::parameterFileExtension =   "params";
 const char* AppSettings::planFileExtension =        "plan";
@@ -78,6 +79,7 @@ AppSettings::AppSettings(QObject* parent)
     , _esriTokenFact                        (NULL)
     , _defaultFirmwareTypeFact              (NULL)
     , _gstDebugFact                         (NULL)
+    , _followTargetFact                     (NULL)
 {
     QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
     qmlRegisterUncreatableType<AppSettings>("QGroundControl.SettingsManager", 1, 0, "AppSettings", "Reference only");
@@ -403,3 +405,13 @@ Fact* AppSettings::defaultFirmwareType(void)
 
     return _defaultFirmwareTypeFact;
 }
+
+Fact* AppSettings::followTarget(void)
+{
+    if (!_followTargetFact) {
+        _followTargetFact = _createSettingsFact(followTargetName);
+    }
+
+    return _followTargetFact;
+}
+

src/Settings/AppSettings.h

@@ -41,6 +41,7 @@ public:
     Q_PROPERTY(Fact* esriToken                          READ esriToken                          CONSTANT)
     Q_PROPERTY(Fact* defaultFirmwareType                READ defaultFirmwareType                CONSTANT)
     Q_PROPERTY(Fact* gstDebug                           READ gstDebug                           CONSTANT)
+    Q_PROPERTY(Fact* followTarget                       READ followTarget                       CONSTANT)
 
     Q_PROPERTY(QString missionSavePath      READ missionSavePath    NOTIFY savePathsChanged)
     Q_PROPERTY(QString parameterSavePath    READ parameterSavePath  NOTIFY savePathsChanged)
@@ -78,6 +79,7 @@ public:
     Fact* esriToken                         (void);
     Fact* defaultFirmwareType               (void);
     Fact* gstDebug                          (void);
+    Fact* followTarget                      (void);
 
     QString missionSavePath     (void);
     QString parameterSavePath   (void);
@@ -112,6 +114,7 @@ public:
     static const char* esriTokenName;
     static const char* defaultFirmwareTypeName;
     static const char* gstDebugName;
+    static const char* followTargetName;
 
     // Application wide file extensions
     static const char* parameterFileExtension;
@@ -161,6 +164,7 @@ private:
     SettingsFact* _esriTokenFact;
     SettingsFact* _defaultFirmwareTypeFact;
     SettingsFact* _gstDebugFact;
+    SettingsFact* _followTargetFact;
 };
 
 #endif

src/ui/preferences/GeneralSettings.qml

@@ -41,6 +41,7 @@ QGCView {
     property real _editFieldWidth:              ScreenTools.defaultFontPixelWidth * 30
     property Fact _mapProvider:                 QGroundControl.settingsManager.flightMapSettings.mapProvider
     property Fact _mapType:                     QGroundControl.settingsManager.flightMapSettings.mapType
+    property Fact _followTarget:                QGroundControl.settingsManager.appSettings.followTarget
     property real _panelWidth:                  _qgcView.width * _internalWidthRatio
 
     readonly property real _internalWidthRatio:          0.8
@@ -245,6 +246,23 @@ QGCView {
                             }
                         }
                         //-----------------------------------------------------------------
+                        //-- Follow Target
+                        Row {
+                            spacing:    ScreenTools.defaultFontPixelWidth
+                            visible:    _followTarget.visible
+                            QGCLabel {
+                                text:               qsTr("Stream GCS Position:")
+                                width:              _labelWidth
+                                anchors.verticalCenter: parent.verticalCenter
+                            }
+                            FactComboBox {
+                                width:      _editFieldWidth
+                                fact:       _followTarget
+                                indexModel: false
+                                anchors.verticalCenter: parent.verticalCenter
+                            }
+                        }
+                        //-----------------------------------------------------------------
                         //-- Audio preferences
                         FactCheckBox {
                             text:       qsTr("Mute all audio output")