Commit d69ef3d4 authored by Jimmy Johnson's avatar Jimmy Johnson

adding follow me

parent 70124a1e
......@@ -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 \
......
......@@ -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 {
......
......@@ -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)
......
......@@ -74,6 +74,7 @@ public:
static const char* rtlFlightMode;
static const char* landingFlightMode;
static const char* rtgsFlightMode;
static const char* followMeFlightMode;
};
#endif
/*=====================================================================
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
/*=====================================================================
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
};
......@@ -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)
......
......@@ -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
......
......@@ -29,6 +29,7 @@
#include "MAVLinkProtocol.h"
#include "UAS.h"
#include "QGCApplication.h"
#include "FollowMe.h"
#ifdef __mobile__
#include "MobileScreenMgr.h"
......
......@@ -35,6 +35,7 @@
class FirmwarePluginManager;
class AutoPilotPluginManager;
class FollowMe;
class JoystickManager;
class QGCApplication;
class MAVLinkProtocol;
......
......@@ -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);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment