From f8e2dbe90e85bbbf63660703fe45648938abac7b Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 21 Sep 2019 11:52:25 -0700 Subject: [PATCH] Plugin'ized follow me support --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 44 ++++ src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 5 +- .../APM/ArduCopterFirmwarePlugin.h | 7 +- .../APM/ArduRoverFirmwarePlugin.h | 3 +- src/FirmwarePlugin/FirmwarePlugin.cc | 25 ++ src/FirmwarePlugin/FirmwarePlugin.h | 7 + src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 1 + src/FlightDisplay/FlightDisplayViewMap.qml | 1 - src/FollowMe/FollowMe.cc | 228 ++++++++---------- src/FollowMe/FollowMe.h | 61 +++-- src/PositionManager/PositionManager.cpp | 2 + src/PositionManager/PositionManager.h | 24 +- src/QGCApplication.cc | 2 + src/QGCApplication.h | 4 + src/Settings/App.SettingsGroup.json | 2 +- src/Vehicle/Vehicle.cc | 9 +- src/Vehicle/Vehicle.h | 2 + 17 files changed, 249 insertions(+), 178 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 64a496278..1ef62b391 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -1086,3 +1086,47 @@ void APMFirmwarePlugin::_handleRCChannelsRaw(Vehicle* vehicle, mavlink_message_t &channels); } +void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities) +{ + if (vehicle->homePosition().isValid()) { + static bool sentOnce = false; + if (!sentOnce) { + sentOnce = true; + qgcApp()->showMessage(tr("Follow failed: Home position not set.")); + } + return; + } + + if (!(estimationCapabilities & (FollowMe::POS | FollowMe::VEL))) { + static bool sentOnce = false; + if (!sentOnce) { + sentOnce = true; + qWarning() << "APMFirmwarePlugin::_sendGCSMotionReport estimateCapabilities" << estimationCapabilities; + qgcApp()->showMessage(tr("Follow failed: Ground station cannot provide required position information.")); + } + return; + } + + MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol(); + + mavlink_global_position_int_t globalPositionInt; + memset(&globalPositionInt, 0, sizeof(globalPositionInt)); + + globalPositionInt.time_boot_ms = static_cast(qgcApp()->msecsSinceBoot()); + globalPositionInt.lat = motionReport.lat_int; + globalPositionInt.lon = motionReport.lon_int; + globalPositionInt.alt = static_cast(motionReport.altMetersAMSL * 1000); // mm + globalPositionInt.relative_alt = static_cast((motionReport.altMetersAMSL - vehicle->homePosition().altitude()) * 1000); // mm + globalPositionInt.vx = static_cast(motionReport.vxMetersPerSec * 100); // cm/sec + globalPositionInt.vy = static_cast(motionReport.vyMetersPerSec * 100); // cm/sec + globalPositionInt.vy = static_cast(motionReport.vzMetersPerSec * 100); // cm/sec + globalPositionInt.hdg = UINT16_MAX; + + mavlink_message_t message; + mavlink_msg_global_position_int_encode_chan(static_cast(mavlinkProtocol->getSystemId()), + static_cast(mavlinkProtocol->getComponentId()), + vehicle->priorityLink()->mavlinkChannel(), + &message, + &globalPositionInt); + vehicle->sendMessageOnLink(vehicle->priorityLink(), message); +} diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index fef3418d7..17aee29d1 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -17,6 +17,7 @@ #include "FirmwarePlugin.h" #include "QGCLoggingCategory.h" #include "APMParameterMetaData.h" +#include "FollowMe.h" #include @@ -109,7 +110,9 @@ public: protected: /// All access to singleton is through stack specific implementation APMFirmwarePlugin(void); - void setSupportedModes(QList supportedModes); + + void setSupportedModes (QList supportedModes); + void _sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities); bool _coaxialMotors; diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index a675e74a4..f6dc8868c 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -64,9 +64,10 @@ public: bool multiRotorCoaxialMotors (Vehicle* vehicle) final; bool multiRotorXConfig (Vehicle* vehicle) final; QString offlineEditingParamFile (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); } - QString pauseFlightMode (void) const override { return QString("Brake"); } - QString landFlightMode (void) const override { return QString("Land"); } - QString takeControlFlightMode (void) const override { return QString("Loiter"); } + QString pauseFlightMode (void) const override { return QStringLiteral("Brake"); } + QString landFlightMode (void) const override { return QStringLiteral("Land"); } + QString takeControlFlightMode (void) const override { return QStringLiteral("Loiter"); } + QString followFlightMode (void) const override { return QStringLiteral("Follow"); } bool vehicleYawsToNextWaypointInMission (const Vehicle* vehicle) const override; QString autoDisarmParameter (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); } bool supportsSmartRTL (void) const override { return true; } diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h index ad17a034c..c71b39567 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h @@ -45,7 +45,8 @@ public: ArduRoverFirmwarePlugin(void); // Overrides from FirmwarePlugin - QString pauseFlightMode (void) const override { return QString("Hold"); } + QString pauseFlightMode (void) const override { return QStringLiteral("Hold"); } + QString followFlightMode (void) const override { return QStringLiteral("Follow"); } void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) final; int remapParamNameHigestMinorVersionNumber (int majorVersionNumber) const final; const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 0b48c7670..6638f8b23 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -909,3 +909,28 @@ QString FirmwarePlugin::gotoFlightMode(void) const { return QString(); } + +void FirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities) +{ + MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol(); + + mavlink_follow_target_t follow_target = {}; + + follow_target.timestamp = qgcApp()->msecsSinceBoot(); + follow_target.est_capabilities = estimationCapabilities; + follow_target.position_cov[0] = static_cast(motionReport.pos_std_dev[0]); + follow_target.position_cov[2] = static_cast(motionReport.pos_std_dev[2]); + follow_target.alt = static_cast(motionReport.altMetersAMSL); + follow_target.lat = motionReport.lat_int; + follow_target.lon = motionReport.lon_int; + follow_target.vel[0] = static_cast(motionReport.vxMetersPerSec); + follow_target.vel[1] = static_cast(motionReport.vyMetersPerSec); + + mavlink_message_t message; + mavlink_msg_follow_target_encode_chan(static_cast(mavlinkProtocol->getSystemId()), + static_cast(mavlinkProtocol->getComponentId()), + vehicle->priorityLink()->mavlinkChannel(), + &message, + &follow_target); + vehicle->sendMessageOnLink(vehicle->priorityLink(), message); +} diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 51739d46e..a6627e61f 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -19,6 +19,7 @@ #include "AutoPilotPlugin.h" #include "GeoFenceManager.h" #include "RallyPointManager.h" +#include "FollowMe.h" #include #include @@ -122,6 +123,9 @@ public: /// Returns the flight mode which the vehicle will be in if it is performing a goto location virtual QString gotoFlightMode(void) const; + /// Returns the flight mode which the vehicle will be for follow me + virtual QString followFlightMode(void) const { return QString(); }; + /// Set guided flight mode virtual void setGuidedMode(Vehicle* vehicle, bool guidedMode); @@ -320,6 +324,9 @@ public: /// @param metaData - MetaData for fact virtual void adjustMetaData(MAV_TYPE vehicleType, FactMetaData* metaData) {Q_UNUSED(vehicleType); Q_UNUSED(metaData);} + /// Sends the appropriate mavlink message for follow me support + virtual void sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities); + // FIXME: Hack workaround for non pluginize FollowMe support static const QString px4FollowMeFlightMode; diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 05a90b66c..cf8d72aeb 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -43,6 +43,7 @@ public: QString landFlightMode (void) const override { return _landingFlightMode; } QString takeControlFlightMode (void) const override { return _manualFlightMode; } QString gotoFlightMode (void) const override { return _holdFlightMode; } + QString followFlightMode (void) const override { return _followMeFlightMode; }; void pauseVehicle (Vehicle* vehicle) override; void guidedModeRTL (Vehicle* vehicle, bool smartRTL) override; void guidedModeLand (Vehicle* vehicle) override; diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 15beb719a..0ab85dfec 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -80,7 +80,6 @@ FlightMap { // When the user pans the map we stop responding to vehicle coordinate updates until the panRecenterTimer fires onUserPannedChanged: { if (userPanned) { - console.log("user panned") userPanned = false _disableVehicleTracking = true panRecenterTimer.restart() diff --git a/src/FollowMe/FollowMe.cc b/src/FollowMe/FollowMe.cc index 750598980..3ca922b1c 100644 --- a/src/FollowMe/FollowMe.cc +++ b/src/FollowMe/FollowMe.cc @@ -20,176 +20,162 @@ #include "AppSettings.h" FollowMe::FollowMe(QGCApplication* app, QGCToolbox* toolbox) - : QGCTool(app, toolbox), estimatation_capabilities(0) + : QGCTool(app, toolbox) { - 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 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(vehicles[i]); - if (vehicle->px4Firmware() && vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) { - _enable(); - return; - } - } - _disable(); - } + connect(&_gcsMotionReportTimer, &QTimer::timeout, this, &FollowMe::_sendGCSMotionReport); + connect(toolbox->settingsManager()->appSettings()->followTarget(), &Fact::rawValueChanged, this, &FollowMe::_settingsChanged); + + _settingsChanged(); } void FollowMe::_settingsChanged() { - 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; - } + _currentMode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt(); + + switch (_currentMode) { + case MODE_NEVER: + disconnect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &FollowMe::_vehicleAdded); + disconnect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved, this, &FollowMe::_vehicleRemoved); + _disableFollowSend(); + break; + case MODE_ALWAYS: + connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &FollowMe::_vehicleAdded); + connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved, this, &FollowMe::_vehicleRemoved); + _enableFollowSend(); + break; + case MODE_FOLLOWME: + connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &FollowMe::_vehicleAdded); + connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved, this, &FollowMe::_vehicleRemoved); + _enableIfVehicleInFollow(); + break; } } -void FollowMe::_enable() +void FollowMe::_enableFollowSend() { - connect(_toolbox->qgcPositionManager(), - &QGCPositionManager::positionInfoUpdated, - this, - &FollowMe::_setGPSLocation); - _gcsMotionReportTimer.setInterval(_toolbox->qgcPositionManager()->updateInterval()); - _gcsMotionReportTimer.start(); + if (!_gcsMotionReportTimer.isActive()) { + _gcsMotionReportTimer.setInterval(qMax(_toolbox->qgcPositionManager()->updateInterval(), 250)); + _gcsMotionReportTimer.start(); + } } -void FollowMe::_disable() +void FollowMe::_disableFollowSend() { - disconnect(_toolbox->qgcPositionManager(), - &QGCPositionManager::positionInfoUpdated, - this, - &FollowMe::_setGPSLocation); - _gcsMotionReportTimer.stop(); + if (_gcsMotionReportTimer.isActive()) { + _gcsMotionReportTimer.stop(); + } } -void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo) +void FollowMe::_sendGCSMotionReport() { + QGeoPositionInfo geoPositionInfo = _toolbox->qgcPositionManager()->geoPositionInfo(); + QGeoCoordinate gcsCoordinate = geoPositionInfo.coordinate(); + if (!geoPositionInfo.isValid()) { - //-- Invalidate cached coordinates - memset(&_motionReport, 0, sizeof(motionReport_s)); - } else { - // get the current location coordinates + return; + } - QGeoCoordinate geoCoordinate = geoPositionInfo.coordinate(); + GCSMotionReport motionReport = {}; + uint8_t estimatation_capabilities = 0; - _motionReport.lat_int = geoCoordinate.latitude() * 1e7; - _motionReport.lon_int = geoCoordinate.longitude() * 1e7; - _motionReport.alt = geoCoordinate.altitude(); + // get the current location coordinates - estimatation_capabilities |= (1 << POS); + motionReport.lat_int = static_cast(gcsCoordinate.latitude() * 1e7); + motionReport.lon_int = static_cast(gcsCoordinate.longitude() * 1e7); + motionReport.altMetersAMSL = gcsCoordinate.altitude(); + estimatation_capabilities |= (1 << POS); - // get the current eph and epv + if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) == true) { + motionReport.headingDegrees = geoPositionInfo.attribute(QGeoPositionInfo::Direction); + } - if(geoPositionInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy) == true) { - _motionReport.pos_std_dev[0] = _motionReport.pos_std_dev[1] = geoPositionInfo.attribute(QGeoPositionInfo::HorizontalAccuracy); - } + // get the current eph and epv - if(geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy) == true) { - _motionReport.pos_std_dev[2] = geoPositionInfo.attribute(QGeoPositionInfo::VerticalAccuracy); - } + if (geoPositionInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) { + motionReport.pos_std_dev[0] = motionReport.pos_std_dev[1] = geoPositionInfo.attribute(QGeoPositionInfo::HorizontalAccuracy); + } - // calculate z velocity if it's available + if (geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) { + motionReport.pos_std_dev[2] = geoPositionInfo.attribute(QGeoPositionInfo::VerticalAccuracy); + } - if(geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalSpeed)) { - _motionReport.vz = geoPositionInfo.attribute(QGeoPositionInfo::VerticalSpeed); - } + // calculate z velocity if it's available + + if (geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalSpeed)) { + motionReport.vzMetersPerSec = geoPositionInfo.attribute(QGeoPositionInfo::VerticalSpeed); + } - // calculate x,y velocity if it's available + // calculate x,y velocity if it's available - if((geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) == true) && - (geoPositionInfo.hasAttribute(QGeoPositionInfo::GroundSpeed) == true)) { + if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) && geoPositionInfo.hasAttribute(QGeoPositionInfo::GroundSpeed) == true) { + estimatation_capabilities |= (1 << VEL); - estimatation_capabilities |= (1 << VEL); + qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction)); + qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed); - qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction)); - qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed); + motionReport.vxMetersPerSec = cos(direction)*velocity; + motionReport.vyMetersPerSec = sin(direction)*velocity; + } else { + motionReport.vxMetersPerSec = 0; + motionReport.vyMetersPerSec = 0; + } - _motionReport.vx = cos(direction)*velocity; - _motionReport.vy = sin(direction)*velocity; + QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles(); - } else { - _motionReport.vx = 0.0f; - _motionReport.vy = 0.0f; + for (int i=0; icount(); i++) { + Vehicle* vehicle = vehicles->value(i); + if (_currentMode == MODE_ALWAYS || (_currentMode == MODE_FOLLOWME && _isFollowFlightMode(vehicle, vehicle->flightMode()))) { + vehicle->firmwarePlugin()->sendGCSMotionReport(vehicle, motionReport, estimatation_capabilities); } } } -void FollowMe::_sendGCSMotionReport() +double FollowMe::_degreesToRadian(double deg) +{ + return deg * M_PI / 180.0; +} + +void FollowMe::_vehicleAdded(Vehicle* vehicle) { + connect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow); + _enableIfVehicleInFollow(); +} + +void FollowMe::_vehicleRemoved(Vehicle* vehicle) +{ + disconnect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow); + _enableIfVehicleInFollow(); +} - //-- Do we have any real data? - if(_motionReport.lat_int == 0 && _motionReport.lon_int == 0 && _motionReport.alt == 0) { +void FollowMe::_enableIfVehicleInFollow(void) +{ + if (_currentMode == MODE_ALWAYS) { + // System always enabled 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.est_capabilities = estimatation_capabilities; - 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; - follow_target.vel[0] = _motionReport.vx; - follow_target.vel[1] = _motionReport.vy; - - for (int i=0; i< vehicles.count(); i++) { - Vehicle* vehicle = qobject_cast(vehicles[i]); - if(_currentMode || vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) { - mavlink_message_t message; - mavlink_msg_follow_target_encode_chan(mavlinkProtocol->getSystemId(), - mavlinkProtocol->getComponentId(), - vehicle->priorityLink()->mavlinkChannel(), - &message, - &follow_target); - vehicle->sendMessageOnLink(vehicle->priorityLink(), message); + // Any vehicle in follow mode will enable the system + QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles(); + + for (int i=0; i< vehicles->count(); i++) { + Vehicle* vehicle = vehicles->value(i); + if (_isFollowFlightMode(vehicle, vehicle->flightMode())) { + _enableFollowSend(); + return; } } + + _disableFollowSend(); } -double FollowMe::_degreesToRadian(double deg) +bool FollowMe::_isFollowFlightMode (Vehicle* vehicle, const QString& flightMode) { - return deg * M_PI / 180.0; + return flightMode.compare(vehicle->followFlightMode()) == 0; } diff --git a/src/FollowMe/FollowMe.h b/src/FollowMe/FollowMe.h index 4b744a538..e17bcab1f 100644 --- a/src/FollowMe/FollowMe.h +++ b/src/FollowMe/FollowMe.h @@ -20,6 +20,8 @@ #include "QGCToolbox.h" #include "MAVLinkProtocol.h" +class Vehicle; + Q_DECLARE_LOGGING_CATEGORY(FollowMeLog) class FollowMe : public QGCTool @@ -29,36 +31,18 @@ class FollowMe : public QGCTool public: FollowMe(QGCApplication* app, QGCToolbox* toolbox); - void setToolbox(QGCToolbox* toolbox) override; - -public slots: - void followMeHandleManager (const QString&); - -private slots: - void _setGPSLocation (QGeoPositionInfo geoPositionInfo); - void _sendGCSMotionReport (); - void _settingsChanged (); - -private: - QElapsedTimer runTime; - QTimer _gcsMotionReportTimer; // Timer to emit motion reports - - 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; + struct GCSMotionReport { + int lat_int; // X Position in WGS84 frame in 1e7 * meters + int lon_int; // Y Position in WGS84 frame in 1e7 * meters + double altMetersAMSL; // Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + double headingDegrees; // Heading in degrees + double vxMetersPerSec; // X velocity in NED frame in meter / s + double vyMetersPerSec; // Y velocity in NED frame in meter / s + double vzMetersPerSec; // Z velocity in NED frame in meter / s + double pos_std_dev[3]; // -1 for unknown + }; // Mavlink defined motion reporting capabilities - enum { POS = 0, VEL = 1, @@ -66,18 +50,27 @@ private: ATT_RATES = 3 }; - uint8_t estimatation_capabilities; - - void _disable (); - void _enable (); + void setToolbox(QGCToolbox* toolbox) override; - double _degreesToRadian(double deg); +private slots: + void _sendGCSMotionReport (void); + void _settingsChanged (void); + void _vehicleAdded (Vehicle* vehicle); + void _vehicleRemoved (Vehicle* vehicle); + void _enableIfVehicleInFollow (void); +private: enum { MODE_NEVER, MODE_ALWAYS, MODE_FOLLOWME }; - uint32_t _currentMode; + void _disableFollowSend (void); + void _enableFollowSend (void); + double _degreesToRadian (double deg); + bool _isFollowFlightMode (Vehicle* vehicle, const QString& flightMode); + + QTimer _gcsMotionReportTimer; + uint32_t _currentMode; }; diff --git a/src/PositionManager/PositionManager.cpp b/src/PositionManager/PositionManager.cpp index 4ed76fdc7..38d36d0b1 100644 --- a/src/PositionManager/PositionManager.cpp +++ b/src/PositionManager/PositionManager.cpp @@ -77,6 +77,8 @@ void QGCPositionManager::setNmeaSourceDevice(QIODevice* device) void QGCPositionManager::_positionUpdated(const QGeoPositionInfo &update) { + _geoPositionInfo = update; + QGeoCoordinate newGCSPosition = QGeoCoordinate(); qreal newGCSHeading = update.attribute(QGeoPositionInfo::Direction); diff --git a/src/PositionManager/PositionManager.h b/src/PositionManager/PositionManager.h index 30c39f363..0f2d3a74c 100644 --- a/src/PositionManager/PositionManager.h +++ b/src/PositionManager/PositionManager.h @@ -35,17 +35,16 @@ public: NmeaGPS }; - QGeoCoordinate gcsPosition(void) { return _gcsPosition; } + QGeoCoordinate gcsPosition (void) { return _gcsPosition; } + qreal gcsHeading (void) { return _gcsHeading; } + QGeoPositionInfo geoPositionInfo (void) const { return _geoPositionInfo; } + void setPositionSource (QGCPositionSource source); + int updateInterval (void) const; + void setNmeaSourceDevice (QIODevice* device); - qreal gcsHeading() { return _gcsHeading; } + // Overrides from QGCTool + void setToolbox(QGCToolbox* toolbox) override; - void setPositionSource(QGCPositionSource source); - - int updateInterval() const; - - void setToolbox(QGCToolbox* toolbox); - - void setNmeaSourceDevice(QIODevice* device); private slots: void _positionUpdated(const QGeoPositionInfo &update); @@ -57,9 +56,10 @@ signals: void positionInfoUpdated(QGeoPositionInfo update); private: - int _updateInterval; - QGeoCoordinate _gcsPosition; - qreal _gcsHeading; + int _updateInterval; + QGeoPositionInfo _geoPositionInfo; + QGeoCoordinate _gcsPosition; + qreal _gcsHeading; QGeoPositionInfoSource* _currentSource; QGeoPositionInfoSource* _defaultSource; diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 0d0d45e93..887b39e45 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -161,6 +161,8 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) , _runningUnitTests (unitTesting) { _app = this; + _msecsElapsedTime.start(); + #ifdef Q_OS_LINUX #ifndef __mobile__ if (!_runningUnitTests) { diff --git a/src/QGCApplication.h b/src/QGCApplication.h index c48edcb48..0e631dc34 100644 --- a/src/QGCApplication.h +++ b/src/QGCApplication.h @@ -22,6 +22,7 @@ #include #include #include +#include #include "LinkConfiguration.h" #include "LinkManager.h" @@ -98,6 +99,8 @@ public: void setLanguage(); QQuickItem* mainRootWindow(); + uint64_t msecsSinceBoot(void) { return _msecsElapsedTime.elapsed(); } + public slots: /// You can connect to this slot to show an information message box from a different thread. void informationMessageBoxOnMainThread(const QString& title, const QString& msg); @@ -192,6 +195,7 @@ private: QTranslator _QGCTranslatorQt; QLocale _locale; bool _error = false; + QElapsedTimer _msecsElapsedTime; static const char* _settingsVersionKey; ///< Settings key which hold settings version static const char* _deleteAllSettingsKey; ///< If this settings key is set on boot, all settings will be deleted diff --git a/src/Settings/App.SettingsGroup.json b/src/Settings/App.SettingsGroup.json index eaa88ac86..7dabbee79 100644 --- a/src/Settings/App.SettingsGroup.json +++ b/src/Settings/App.SettingsGroup.json @@ -212,7 +212,7 @@ "type": "uint32", "enumStrings": "Never,Always,When in Follow Me Flight Mode", "enumValues": "0,1,2", - "defaultValue": 0 + "defaultValue": 2 }, { "name": "apmStartMavlinkStreams", diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 23bc05d47..89e8065ce 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -28,7 +28,6 @@ #include "ParameterManager.h" #include "QGCApplication.h" #include "QGCImageProvider.h" -#include "FollowMe.h" #include "MissionCommandTree.h" #include "QGroundControlQmlGlobal.h" #include "SettingsManager.h" @@ -245,9 +244,6 @@ Vehicle::Vehicle(LinkInterface* link, _commonInit(); _autopilotPlugin = _firmwarePlugin->autopilotPlugin(this); - // connect this vehicle to the follow me handle manager - connect(this, &Vehicle::flightModeChanged,_toolbox->followMe(), &FollowMe::followMeHandleManager); - // PreArm Error self-destruct timer connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout); _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs); @@ -3699,6 +3695,11 @@ QString Vehicle::takeControlFlightMode(void) const return _firmwarePlugin->takeControlFlightMode(); } +QString Vehicle::followFlightMode(void) const +{ + return _firmwarePlugin->followFlightMode(); +} + QString Vehicle::vehicleImageOpaque() const { if(_firmwarePlugin) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 2876044cc..97ac0af04 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -606,6 +606,7 @@ public: Q_PROPERTY(bool supportsSmartRTL READ supportsSmartRTL CONSTANT) Q_PROPERTY(QString landFlightMode READ landFlightMode CONSTANT) Q_PROPERTY(QString takeControlFlightMode READ takeControlFlightMode CONSTANT) + Q_PROPERTY(QString followFlightMode READ followFlightMode CONSTANT) Q_PROPERTY(QString firmwareTypeString READ firmwareTypeString NOTIFY firmwareTypeChanged) Q_PROPERTY(QString vehicleTypeString READ vehicleTypeString NOTIFY vehicleTypeChanged) Q_PROPERTY(QString vehicleImageOpaque READ vehicleImageOpaque CONSTANT) @@ -942,6 +943,7 @@ public: bool supportsSmartRTL () const; QString landFlightMode () const; QString takeControlFlightMode () const; + QString followFlightMode () const; double defaultCruiseSpeed () const { return _defaultCruiseSpeed; } double defaultHoverSpeed () const { return _defaultHoverSpeed; } QString firmwareTypeString () const; -- 2.22.0