From 893f4194da6e720caa43c50ecfcedf2173e7b269 Mon Sep 17 00:00:00 2001
From: Gus Grubba <mavlink@grubba.com>
Date: Thu, 26 Apr 2018 10:10:28 -0400
Subject: [PATCH] Allow manual control of when to send FOLLOW_TARGET messages.

---
 src/FollowMe/FollowMe.cc | 36 +++++++++++++++++++++++-------------
 src/FollowMe/FollowMe.h  | 11 ++++++++---
 2 files changed, 31 insertions(+), 16 deletions(-)

diff --git a/src/FollowMe/FollowMe.cc b/src/FollowMe/FollowMe.cc
index ae18e8ecd..14c3017ce 100644
--- a/src/FollowMe/FollowMe.cc
+++ b/src/FollowMe/FollowMe.cc
@@ -19,37 +19,48 @@
 
 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);
     connect(&_gcsMotionReportTimer, &QTimer::timeout, this, &FollowMe::_sendGCSMotionReport);
 }
 
 void FollowMe::followMeHandleManager(const QString&)
 {
-    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();
-            return;
+    if(!_manualControl) {
+        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());
+                return;
+            }
         }
+        _disable();
     }
+}
 
+void FollowMe::manualEnable(int interval)
+{
+    _manualControl = true;
+    _enable(interval);
+}
+
+void FollowMe::manualDisable()
+{
+    _manualControl = false;
     _disable();
 }
 
-void FollowMe::_enable()
+void FollowMe::_enable(int interval)
 {
     connect(_toolbox->qgcPositionManager(),
             SIGNAL(positionInfoUpdated(QGeoPositionInfo)),
             this,
             SLOT(_setGPSLocation(QGeoPositionInfo)));
-
-    _gcsMotionReportTimer.setInterval(_toolbox->qgcPositionManager()->updateInterval());
+    _gcsMotionReportTimer.setInterval(interval);
     _gcsMotionReportTimer.start();
 }
 
@@ -59,7 +70,6 @@ void FollowMe::_disable()
                SIGNAL(positionInfoUpdated(QGeoPositionInfo)),
                this,
                SLOT(_setGPSLocation(QGeoPositionInfo)));
-
     _gcsMotionReportTimer.stop();
 }
 
@@ -133,7 +143,7 @@ void FollowMe::_sendGCSMotionReport(void)
 
     for (int i=0; i< vehicles.count(); i++) {
         Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
-        if(vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
+        if(_manualControl || vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
             mavlink_message_t message;
             mavlink_msg_follow_target_encode_chan(mavlinkProtocol->getSystemId(),
                                                   mavlinkProtocol->getComponentId(),
diff --git a/src/FollowMe/FollowMe.h b/src/FollowMe/FollowMe.h
index 541df1112..ded14153e 100644
--- a/src/FollowMe/FollowMe.h
+++ b/src/FollowMe/FollowMe.h
@@ -29,6 +29,9 @@ class FollowMe : public QGCTool
 public:
     FollowMe(QGCApplication* app, QGCToolbox* toolbox);
 
+    void    manualEnable    (int interval);
+    void    manualDisable   ();
+
 public slots:
     void followMeHandleManager(const QString&);
 
@@ -65,8 +68,10 @@ private:
 
     uint8_t estimatation_capabilities;
 
-    void _disable();
-    void _enable();
+    void    _disable    ();
+    void    _enable     (int interval);
+
+    double  _degreesToRadian(double deg);
 
-    double _degreesToRadian(double deg);
+    bool    _manualControl;
 };
-- 
2.22.0