diff --git a/src/FollowMe/FollowMe.cc b/src/FollowMe/FollowMe.cc index 14c3017ce7928e47bffb621744d7b774cea15221..e7f4593c127f747b7363520fd6b98a132ee82794 100644 --- a/src/FollowMe/FollowMe.cc +++ b/src/FollowMe/FollowMe.cc @@ -16,25 +16,37 @@ #include "FollowMe.h" #include "Vehicle.h" #include "PositionManager.h" +#include "SettingsManager.h" +#include "AppSettings.h" 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); +} + +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(!_manualControl) { + //-- 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(_toolbox->qgcPositionManager()->updateInterval()); + _enable(); return; } } @@ -42,25 +54,40 @@ void FollowMe::followMeHandleManager(const QString&) } } -void FollowMe::manualEnable(int interval) +void FollowMe::_settingsChanged() { - _manualControl = true; - _enable(interval); -} - -void FollowMe::manualDisable() -{ - _manualControl = false; - _disable(); + 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; + } + } } -void FollowMe::_enable(int interval) +void FollowMe::_enable() { connect(_toolbox->qgcPositionManager(), SIGNAL(positionInfoUpdated(QGeoPositionInfo)), this, SLOT(_setGPSLocation(QGeoPositionInfo))); - _gcsMotionReportTimer.setInterval(interval); + _gcsMotionReportTimer.setInterval(_toolbox->qgcPositionManager()->updateInterval()); _gcsMotionReportTimer.start(); } @@ -75,15 +102,17 @@ void FollowMe::_disable() void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo) { - if (geoPositionInfo.isValid()) - { + if (!geoPositionInfo.isValid()) { + //-- Invalidate cached coordinates + memset(&_motionReport, 0, sizeof(motionReport_s)); + } else { // 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(); + _motionReport.lat_int = geoCoordinate.latitude() * 1e7; + _motionReport.lon_int = geoCoordinate.longitude() * 1e7; + _motionReport.alt = geoCoordinate.altitude(); estimatation_capabilities |= (1 << POS); @@ -111,7 +140,7 @@ void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo) estimatation_capabilities |= (1 << VEL); qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction)); - qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed); + qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed); _motionReport.vx = cos(direction)*velocity; _motionReport.vy = sin(direction)*velocity; @@ -123,15 +152,20 @@ void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo) } } -void FollowMe::_sendGCSMotionReport(void) +void FollowMe::_sendGCSMotionReport() { - QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles(); + + //-- Do we have any real data? + if(_motionReport.lat_int == 0 && _motionReport.lon_int == 0 && _motionReport.alt == 0) { + 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.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]; @@ -143,7 +177,7 @@ void FollowMe::_sendGCSMotionReport(void) for (int i=0; i< vehicles.count(); i++) { Vehicle* vehicle = qobject_cast(vehicles[i]); - if(_manualControl || vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) { + if(_currentMode || 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 ded14153ec272be55b0aabedf0242eda23c6bd4e..4b744a538343fb32e04e623beb6b8f7f1afa7d2d 100644 --- a/src/FollowMe/FollowMe.h +++ b/src/FollowMe/FollowMe.h @@ -29,15 +29,15 @@ class FollowMe : public QGCTool public: FollowMe(QGCApplication* app, QGCToolbox* toolbox); - void manualEnable (int interval); - void manualDisable (); + void setToolbox(QGCToolbox* toolbox) override; public slots: - void followMeHandleManager(const QString&); + void followMeHandleManager (const QString&); private slots: - void _setGPSLocation(QGeoPositionInfo geoPositionInfo); - void _sendGCSMotionReport(void); + void _setGPSLocation (QGeoPositionInfo geoPositionInfo); + void _sendGCSMotionReport (); + void _settingsChanged (); private: QElapsedTimer runTime; @@ -69,9 +69,15 @@ private: uint8_t estimatation_capabilities; void _disable (); - void _enable (int interval); + void _enable (); double _degreesToRadian(double deg); - bool _manualControl; + enum { + MODE_NEVER, + MODE_ALWAYS, + MODE_FOLLOWME + }; + + uint32_t _currentMode; }; diff --git a/src/Settings/App.SettingsGroup.json b/src/Settings/App.SettingsGroup.json index 81520a00d83d844c8af5b5436cfe0616471935de..8e60817654335ff3cd472879bc6a075039e1d178 100644 --- a/src/Settings/App.SettingsGroup.json +++ b/src/Settings/App.SettingsGroup.json @@ -183,5 +183,13 @@ "shortDescription": "Default firmware type for flashing", "type": "uint32", "defaultValue": 12 +}, +{ + "name": "FollowTarget", + "shortDescription": "Stream GCS' coordinates to Autopilot", + "type": "uint32", + "enumStrings": "Never,Always,When in Follow Me Flight Mode", + "enumValues": "0,1,2", + "defaultValue": 0 } ] diff --git a/src/Settings/AppSettings.cc b/src/Settings/AppSettings.cc index 1b9c7364b078c44df2924b74c5b1dd9b2713b768..56177c3196670e61d4d417281d1809b7675dd4f5 100644 --- a/src/Settings/AppSettings.cc +++ b/src/Settings/AppSettings.cc @@ -37,6 +37,7 @@ const char* AppSettings::mapboxTokenName = "MapboxT const char* AppSettings::esriTokenName = "EsriToken"; const char* AppSettings::defaultFirmwareTypeName = "DefaultFirmwareType"; const char* AppSettings::gstDebugName = "GstreamerDebugLevel"; +const char* AppSettings::followTargetName = "FollowTarget"; const char* AppSettings::parameterFileExtension = "params"; const char* AppSettings::planFileExtension = "plan"; @@ -78,6 +79,7 @@ AppSettings::AppSettings(QObject* parent) , _esriTokenFact (NULL) , _defaultFirmwareTypeFact (NULL) , _gstDebugFact (NULL) + , _followTargetFact (NULL) { QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); qmlRegisterUncreatableType("QGroundControl.SettingsManager", 1, 0, "AppSettings", "Reference only"); @@ -403,3 +405,13 @@ Fact* AppSettings::defaultFirmwareType(void) return _defaultFirmwareTypeFact; } + +Fact* AppSettings::followTarget(void) +{ + if (!_followTargetFact) { + _followTargetFact = _createSettingsFact(followTargetName); + } + + return _followTargetFact; +} + diff --git a/src/Settings/AppSettings.h b/src/Settings/AppSettings.h index 108f6658bde17c001c54570888109deaaf40b403..d1a7e2fbbf38c301f5012d425d9dfb83ee4a819f 100644 --- a/src/Settings/AppSettings.h +++ b/src/Settings/AppSettings.h @@ -41,6 +41,7 @@ public: Q_PROPERTY(Fact* esriToken READ esriToken CONSTANT) Q_PROPERTY(Fact* defaultFirmwareType READ defaultFirmwareType CONSTANT) Q_PROPERTY(Fact* gstDebug READ gstDebug CONSTANT) + Q_PROPERTY(Fact* followTarget READ followTarget CONSTANT) Q_PROPERTY(QString missionSavePath READ missionSavePath NOTIFY savePathsChanged) Q_PROPERTY(QString parameterSavePath READ parameterSavePath NOTIFY savePathsChanged) @@ -78,6 +79,7 @@ public: Fact* esriToken (void); Fact* defaultFirmwareType (void); Fact* gstDebug (void); + Fact* followTarget (void); QString missionSavePath (void); QString parameterSavePath (void); @@ -112,6 +114,7 @@ public: static const char* esriTokenName; static const char* defaultFirmwareTypeName; static const char* gstDebugName; + static const char* followTargetName; // Application wide file extensions static const char* parameterFileExtension; @@ -161,6 +164,7 @@ private: SettingsFact* _esriTokenFact; SettingsFact* _defaultFirmwareTypeFact; SettingsFact* _gstDebugFact; + SettingsFact* _followTargetFact; }; #endif diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index e74cfe334f5cfb5fe4d05d06a2c5604a449a5b41..f302b3439b2a1275992a7a2af617b12078733d02 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -41,6 +41,7 @@ QGCView { property real _editFieldWidth: ScreenTools.defaultFontPixelWidth * 30 property Fact _mapProvider: QGroundControl.settingsManager.flightMapSettings.mapProvider property Fact _mapType: QGroundControl.settingsManager.flightMapSettings.mapType + property Fact _followTarget: QGroundControl.settingsManager.appSettings.followTarget property real _panelWidth: _qgcView.width * _internalWidthRatio readonly property real _internalWidthRatio: 0.8 @@ -245,6 +246,23 @@ QGCView { } } //----------------------------------------------------------------- + //-- Follow Target + Row { + spacing: ScreenTools.defaultFontPixelWidth + visible: _followTarget.visible + QGCLabel { + text: qsTr("Stream GCS Position:") + width: _labelWidth + anchors.verticalCenter: parent.verticalCenter + } + FactComboBox { + width: _editFieldWidth + fact: _followTarget + indexModel: false + anchors.verticalCenter: parent.verticalCenter + } + } + //----------------------------------------------------------------- //-- Audio preferences FactCheckBox { text: qsTr("Mute all audio output")