diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index d7697da59f2bbdbdf06eaaa3f05350923df7c4fe..9dd7fb8dca561eb9321b18190f7f3d392b4b4d11 100644 --- a/qgroundcontrol.pro +++ b/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 \ diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index 5343e2288c29c764623268d3b1663833df38a8ce..5afedc3cae236349ff44967296b50078e635ce30 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/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 { diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 553192abef81a1057bddc3fe913ca57e82c8bd11..1c15a1091796ec3005fac9f299ec923055286806 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/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 PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 37c588e57ddfc5a7c3b21a486446941d23c4003c..2185809cddf6831b57dcf182b7fddc1d98d9396a 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/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 diff --git a/src/FollowMe/FollowMe.cc b/src/FollowMe/FollowMe.cc new file mode 100644 index 0000000000000000000000000000000000000000..bf503ab3215f9dcdcf45ba5d41a5bdcc8a95a476 --- /dev/null +++ b/src/FollowMe/FollowMe.cc @@ -0,0 +1,218 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2016 QGROUNDCONTROL PROJECT + + 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 . + + ======================================================================*/ + +#include +#include + +#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(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(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 diff --git a/src/FollowMe/FollowMe.h b/src/FollowMe/FollowMe.h new file mode 100644 index 0000000000000000000000000000000000000000..fc59b1037ddfcc102d95303ee57ab854f86fdaeb --- /dev/null +++ b/src/FollowMe/FollowMe.h @@ -0,0 +1,107 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2016 QGROUNDCONTROL PROJECT + + 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 . + + ======================================================================*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#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 +}; diff --git a/src/QGCToolbox.cc b/src/QGCToolbox.cc index fa07e45390253bce5f607dda35b682b189e58b84..fcda047d231c7956f3e6060ed41ba382a5195621 100644 --- a/src/QGCToolbox.cc +++ b/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) diff --git a/src/QGCToolbox.h b/src/QGCToolbox.h index 348a0b7b0c87c8fd828093662891d53d961f20c7..a37f124bf922829e5e6f6fd7e2ab267ed27fded7 100644 --- a/src/QGCToolbox.h +++ b/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 diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 9f7a9a9dbf2fe1df6d79f47834bee3d642a19a9e..9e2cd70ed1fb276611d1a4db9af661b73691a7dc 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/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" diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index 31fb92fba1764bd37cbd69093a9afe377b107163..214e13631908f0b45d543bf04107da9bb85609b1 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -35,6 +35,7 @@ class FirmwarePluginManager; class AutoPilotPluginManager; +class FollowMe; class JoystickManager; class QGCApplication; class MAVLinkProtocol; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f1b63d57c6f47d6f2d54688d1e884bda33526980..21dcec3beb29b5c446894eea0938cf85e1a712c6 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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);