Merge pull request #3089 from catch-twenty-two/master

adding follow me

qgroundcontrol.pro

@@ -187,6 +187,7 @@ INCLUDEPATH += \
     src/FlightMap/Widgets \
     src/input \
     src/Joystick \
+    src/FollowMe \
     src/lib/qmapcontrol \
     src/MissionEditor \
     src/MissionManager \
@@ -251,6 +252,7 @@ HEADERS += \
     src/HomePositionManager.h \
     src/Joystick/Joystick.h \
     src/Joystick/JoystickManager.h \
+    src/FollowMe/FollowMe.h \
     src/JsonHelper.h \
     src/LogCompressor.h \
     src/MG.h \
@@ -393,6 +395,7 @@ SOURCES += \
     src/Joystick/Joystick.cc \
     src/Joystick/JoystickManager.cc \
     src/JsonHelper.cc \
+    src/FollowMe/FollowMe.cc \
     src/LogCompressor.cc \
     src/main.cc \
     src/MissionManager/MissionCommandList.cc \

src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc

@@ -54,7 +54,8 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
     PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
     PX4_CUSTOM_SUB_MODE_AUTO_RTL,
     PX4_CUSTOM_SUB_MODE_AUTO_LAND,
-    PX4_CUSTOM_SUB_MODE_AUTO_RTGS
+    PX4_CUSTOM_SUB_MODE_AUTO_RTGS,
+    PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_ME
 };
 
 union px4_custom_mode {

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -48,7 +48,8 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
     PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
     PX4_CUSTOM_SUB_MODE_AUTO_RTL,
     PX4_CUSTOM_SUB_MODE_AUTO_LAND,
-    PX4_CUSTOM_SUB_MODE_AUTO_RTGS
+    PX4_CUSTOM_SUB_MODE_AUTO_RTGS,
+    PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_ME
 };
 
 union px4_custom_mode {
@@ -82,6 +83,7 @@ const char* PX4FirmwarePlugin::missionFlightMode =      "Mission";
 const char* PX4FirmwarePlugin::rtlFlightMode =          "Return To Land";
 const char* PX4FirmwarePlugin::landingFlightMode =      "Landing";
 const char* PX4FirmwarePlugin::rtgsFlightMode =         "Return, Link Loss";
+const char* PX4FirmwarePlugin::followMeFlightMode =     "Follow Me";
 
 /// Tranlates from PX4 custom modes to flight mode names
 
@@ -100,6 +102,7 @@ static const struct Modes2Name rgModes2Name[] = {
     { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTL,       PX4FirmwarePlugin::rtlFlightMode,           true },
     { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LAND,      PX4FirmwarePlugin::landingFlightMode,       false },
     { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTGS,      PX4FirmwarePlugin::rtgsFlightMode,          false },
+    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_ME, PX4FirmwarePlugin::followMeFlightMode,      true },
 };
 
 QList<VehicleComponent*> PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -74,6 +74,7 @@ public:
     static const char* rtlFlightMode;
     static const char* landingFlightMode;
     static const char* rtgsFlightMode;
+    static const char* followMeFlightMode;
 };
 
 #endif

src/FollowMe/FollowMe.cc

+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+ 
+ (c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ 
+ This file is part of the QGROUNDCONTROL project
+ 
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ 
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+ 
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+ 
+ ======================================================================*/
+
+#include <QElapsedTimer>
+#include <cmath>
+
+#include "MultiVehicleManager.h"
+#include "PX4FirmwarePlugin.h"
+#include "MAVLinkProtocol.h"
+#include "FollowMe.h"
+#include "Vehicle.h"
+
+#ifdef QT_QML_DEBUG
+FollowMe::simulated_motion_s FollowMe::_simulated_motion[4] = {{0,500},{500,0},{0, -500},{-500, 0}};
+#endif
+
+FollowMe::FollowMe(QGCApplication* app)
+    : QGCTool(app),
+      _followMeStr(PX4FirmwarePlugin::followMeFlightMode)
+{
+
+#ifdef QT_QML_DEBUG
+    _simulate_motion_timer = 0;
+    _simulate_motion_index = 0;
+    _simulate_motion = false;
+#endif
+
+    memset(&_motionReport, 0, sizeof(motionReport_s));
+
+    // set up the QT position connection slot
+
+    _locationInfo = QGeoPositionInfoSource::createDefaultSource(this);
+
+    if(_locationInfo != 0) {
+
+        _locationInfo->setPreferredPositioningMethods(QGeoPositionInfoSource::SatellitePositioningMethods);
+        _locationInfo->setUpdateInterval(_locationInfo->minimumUpdateInterval());
+        connect(_locationInfo, SIGNAL(positionUpdated(QGeoPositionInfo)), this, SLOT(_setGPSLocation(QGeoPositionInfo)));
+
+        // set up the mavlink motion report timer`
+
+        _gcsMotionReportTimer.setInterval(_locationInfo->minimumUpdateInterval());
+        _gcsMotionReportTimer.setSingleShot(false);
+        connect(&_gcsMotionReportTimer, &QTimer::timeout, this, &FollowMe::_sendGCSMotionReport);
+
+        runTime.start();
+    }
+}
+
+FollowMe::~FollowMe()
+{
+    disable();
+}
+
+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->flightMode().compare(_followMeStr, Qt::CaseInsensitive) == 0) {
+            enable();
+            return;
+        }
+    }
+
+    disable();
+}
+
+void FollowMe::enable()
+{
+    if(_locationInfo != 0) {
+        _locationInfo->startUpdates();
+        _gcsMotionReportTimer.start();
+    }
+}
+
+void FollowMe::disable()
+{
+    if(_locationInfo != 0) {
+        _locationInfo->stopUpdates();
+        _gcsMotionReportTimer.stop();
+    }
+}
+
+void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo)
+{
+    if (geoPositionInfo.isValid())
+    {
+        // 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();
+
+#ifdef QT_QML_DEBUG
+        if(_simulate_motion == true) {
+            _motionReport.lat_int = 47.3977420*1e7;
+            _motionReport.lon_int = 8.5455941*1e7;
+            _motionReport.alt = 488.00;
+        }
+#endif
+        // get the current eph and epv
+
+        if(geoPositionInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy) == true) {
+            _motionReport.pos_std_dev[0] = _motionReport.pos_std_dev[1] = geoPositionInfo.attribute(QGeoPositionInfo::HorizontalAccuracy);
+        }
+
+        if(geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy) == true) {
+            _motionReport.pos_std_dev[2] = geoPositionInfo.attribute(QGeoPositionInfo::VerticalAccuracy);
+        }
+
+        // calculate z velocity if it's availible
+
+        if(geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalSpeed)) {
+            _motionReport.vz = geoPositionInfo.attribute(QGeoPositionInfo::VerticalSpeed);
+        }
+
+        // calculate x,y velocity if it's availible
+
+        if((geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction)   == true) &&
+           (geoPositionInfo.hasAttribute(QGeoPositionInfo::GroundSpeed) == true)) {
+
+            qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction));
+            qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
+
+            _motionReport.vx = cos(direction)*velocity;
+            _motionReport.vy = sin(direction)*velocity;
+        }
+    }
+}
+
+void FollowMe::_sendGCSMotionReport(void)
+{
+    QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles();
+    MAVLinkProtocol* mavlinkProtocol = _toolbox->mavlinkProtocol();
+    mavlink_follow_target_t follow_target;
+
+    memset(&follow_target, 0, sizeof(mavlink_follow_target_t));
+
+    follow_target.timestamp = runTime.nsecsElapsed()*1e-6;
+    follow_target.est_capabilities = (1 << POS);
+    follow_target.position_cov[0] = _motionReport.pos_std_dev[0];
+    follow_target.position_cov[2] = _motionReport.pos_std_dev[2];
+    follow_target.alt = _motionReport.alt;
+    follow_target.lat = _motionReport.lat_int;
+    follow_target.lon = _motionReport.lon_int;
+
+#ifdef QT_QML_DEBUG
+    if(_simulate_motion == true) {
+        _createSimulatedMotion(follow_target);
+    }
+#endif
+
+    for (int i=0; i< vehicles.count(); i++) {
+        Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
+        if(vehicle->flightMode().compare(_followMeStr, Qt::CaseInsensitive) == 0) {
+            mavlink_message_t message;
+            mavlink_msg_follow_target_encode(mavlinkProtocol->getSystemId(),
+                                             mavlinkProtocol->getComponentId(),
+                                             &message,
+                                             &follow_target);
+            vehicle->sendMessage(message);
+        }
+    }
+}
+
+double FollowMe::_degreesToRadian(double deg)
+{
+    return deg * M_PI / 180.0;
+}
+
+#ifdef QT_QML_DEBUG
+void FollowMe::_createSimulatedMotion(mavlink_follow_target_t & follow_target)
+{
+    static int f_lon = 0;
+    static int f_lat = 0;
+    static float rot = 0;
+
+    rot += (float) .1;
+
+    if(!(_simulate_motion_timer++%50)) {
+        _simulate_motion_index++;
+        if(_simulate_motion_index > 3) {
+            _simulate_motion_index = 0;
+        }
+    }
+
+    f_lon = f_lon + _simulated_motion[_simulate_motion_index].lon*sin(rot);
+    f_lat = f_lat + _simulated_motion[_simulate_motion_index].lat;
+
+    follow_target.alt = _motionReport.alt;
+    follow_target.lat = _motionReport.lat_int + f_lon;
+    follow_target.lon = _motionReport.lon_int + f_lat;
+}
+#endif

src/FollowMe/FollowMe.h

+/*=====================================================================
+ 
+ QGroundControl Open Source Ground Control Station
+ 
+ (c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ 
+ This file is part of the QGROUNDCONTROL project
+ 
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ 
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+ 
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+ 
+ ======================================================================*/
+
+#pragma once
+
+#include <QTimer>
+#include <QObject>
+#include <QThread>
+#include <QGeoPositionInfo>
+#include <QGeoPositionInfoSource>
+#include <QElapsedTimer>
+
+#include "QGCLoggingCategory.h"
+#include "QGCToolbox.h"
+#include "MAVLinkProtocol.h"
+
+Q_DECLARE_LOGGING_CATEGORY(FollowMeLog)
+
+class FollowMe : public QGCTool
+{
+    Q_OBJECT
+
+public:
+    FollowMe(QGCApplication* app);
+    ~FollowMe();
+
+public slots:
+    void followMeHandleManager(const QString&);
+
+private slots:
+    void _setGPSLocation(QGeoPositionInfo geoPositionInfo);
+    void _sendGCSMotionReport(void);
+
+private:
+    QGeoPositionInfoSource * _locationInfo;
+    QElapsedTimer runTime;
+
+    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;
+
+    QString _followMeStr;
+
+    QTimer _gcsMotionReportTimer;        ///< Timer to emit motion reports
+    double _degreesToRadian(double deg);
+
+    void disable();
+    void enable();
+
+    // Mavlink defined motion reporting capabilities
+
+    enum {
+        POS = 0,
+        VEL = 1,
+        ACCEL = 2,
+        ATT_RATES = 3
+    };
+
+#ifdef QT_QML_DEBUG
+
+    // items for simulating QGC movment in jMAVSIM
+
+    struct simulated_motion_s {
+        int lon;
+        int lat;
+    };
+
+    static simulated_motion_s _simulated_motion[4];
+
+    int _simulate_motion_timer;
+    int _simulate_motion_index;
+
+    bool _simulate_motion;
+
+    void _createSimulatedMotion(mavlink_follow_target_t & follow_target);
+#endif
+};

src/QGCToolbox.cc

@@ -35,6 +35,7 @@
 #include "QGCImageProvider.h"
 #include "UASMessageHandler.h"
 #include "QGCMapEngineManager.h"
+#include "FollowMe.h"
 
 QGCToolbox::QGCToolbox(QGCApplication* app)
     : _audioOutput(NULL)
@@ -51,6 +52,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app)
     , _multiVehicleManager(NULL)
     , _mapEngineManager(NULL)
     , _uasMessageHandler(NULL)
+    , _followMe(NULL)
 {
     _audioOutput =              new GAudioOutput(app);
     _autopilotPluginManager =   new AutoPilotPluginManager(app);
@@ -66,6 +68,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app)
     _multiVehicleManager =      new MultiVehicleManager(app);
     _mapEngineManager =       new QGCMapEngineManager(app);
     _uasMessageHandler =        new UASMessageHandler(app);
+    _followMe =                 new FollowMe(app);
 
     _audioOutput->setToolbox(this);
     _autopilotPluginManager->setToolbox(this);
@@ -81,6 +84,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app)
     _multiVehicleManager->setToolbox(this);
     _mapEngineManager->setToolbox(this);
     _uasMessageHandler->setToolbox(this);
+    _followMe->setToolbox(this);
 }
 
 QGCToolbox::~QGCToolbox()
@@ -98,6 +102,7 @@ QGCToolbox::~QGCToolbox()
     delete _mapEngineManager;
     delete _multiVehicleManager;
     delete _uasMessageHandler;
+    delete _followMe;
 }
 
 QGCTool::QGCTool(QGCApplication* app)

src/QGCToolbox.h

@@ -33,6 +33,7 @@ class FlightMapSettings;
 class GAudioOutput;
 class HomePositionManager;
 class JoystickManager;
+class FollowMe;
 class LinkManager;
 class MAVLinkProtocol;
 class MissionCommands;
@@ -62,6 +63,7 @@ public:
     QGCMapEngineManager*        mapEngineManager(void)          { return _mapEngineManager; }
     QGCImageProvider*           imageProvider()                 { return _imageProvider; }
     UASMessageHandler*          uasMessageHandler(void)         { return _uasMessageHandler; }
+    FollowMe*                   followMe(void)                  { return _followMe; }
 
 private:
     GAudioOutput*               _audioOutput;
@@ -78,6 +80,7 @@ private:
     MultiVehicleManager*        _multiVehicleManager;
     QGCMapEngineManager*         _mapEngineManager;
     UASMessageHandler*          _uasMessageHandler;
+    FollowMe*                   _followMe;
 };
 
 /// This is the base class for all tools

src/Vehicle/MultiVehicleManager.cc

@@ -29,6 +29,7 @@
 #include "MAVLinkProtocol.h"
 #include "UAS.h"
 #include "QGCApplication.h"
+#include "FollowMe.h"
 
 #ifdef __mobile__
 #include "MobileScreenMgr.h"

src/Vehicle/MultiVehicleManager.h

@@ -35,6 +35,7 @@
 
 class FirmwarePluginManager;
 class AutoPilotPluginManager;
+class FollowMe;
 class JoystickManager;
 class QGCApplication;
 class MAVLinkProtocol;

src/Vehicle/Vehicle.cc

@@ -35,6 +35,7 @@
 #include "QGCApplication.h"
 #include "QGCImageProvider.h"
 #include "GAudioOutput.h"
+#include "FollowMe.h"
 
 QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")
 
@@ -143,7 +144,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
     connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_handleFlightModeChanged);
     connect(this, &Vehicle::armedChanged,               this, &Vehicle::_announceArmedChanged);
-
     _uas = new UAS(_mavlink, this, _firmwarePluginManager);
 
     setLatitude(_uas->getLatitude());
@@ -160,6 +160,9 @@ Vehicle::Vehicle(LinkInterface*             link,
     connect(_autopilotPlugin, &AutoPilotPlugin::parametersReadyChanged,     this, &Vehicle::_parametersReady);
     connect(_autopilotPlugin, &AutoPilotPlugin::missingParametersChanged,   this, &Vehicle::missingParametersChanged);
 
+    // connect this vehicle to the follow me handle manager
+    connect(this, &Vehicle::flightModeChanged,qgcApp()->toolbox()->followMe(), &FollowMe::followMeHandleManager);
+
     // Refresh timer
     connect(_refreshTimer, &QTimer::timeout, this, &Vehicle::_checkUpdate);
     _refreshTimer->setInterval(UPDATE_TIMER);