From c9dab611a0663797d335c4fc8bcf9ab4af31ed3a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 1 Sep 2016 11:25:40 -0700 Subject: [PATCH] PX4 GeoFence plugin Also change Plan Element selection ui --- qgroundcontrol.pro | 2 + src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 52 ++++---- src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc | 85 +++++++++++++ src/FirmwarePlugin/PX4/PX4GeoFenceManager.h | 45 +++++++ src/MissionEditor/GeoFenceEditor.qml | 2 +- src/MissionEditor/MissionEditor.qml | 125 ++++++++++--------- src/MissionManager/GeoFenceController.cc | 6 +- src/comm/LinkManager.cc | 2 +- 8 files changed, 229 insertions(+), 90 deletions(-) create mode 100644 src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc create mode 100644 src/FirmwarePlugin/PX4/PX4GeoFenceManager.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 0f8f6abde..a72eec99b 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -700,6 +700,7 @@ HEADERS+= \ src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h \ src/FirmwarePlugin/PX4/px4_custom_mode.h \ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \ + src/FirmwarePlugin/PX4/PX4GeoFenceManager.h \ src/FirmwarePlugin/PX4/PX4ParameterMetaData.h \ src/Vehicle/MultiVehicleManager.h \ src/Vehicle/Vehicle.h \ @@ -761,6 +762,7 @@ SOURCES += \ src/FirmwarePlugin/FirmwarePlugin.cc \ src/FirmwarePlugin/FirmwarePluginManager.cc \ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \ + src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc \ src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc \ src/Vehicle/MultiVehicleManager.cc \ src/Vehicle/Vehicle.cc \ diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 47feadbb3..7bed87dde 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -17,6 +17,7 @@ #include "FirmwarePlugin.h" #include "ParameterLoader.h" #include "PX4ParameterMetaData.h" +#include "PX4GeoFenceManager.h" class PX4FirmwarePlugin : public FirmwarePlugin { @@ -30,31 +31,32 @@ public: QList componentsForVehicle(AutoPilotPlugin* vehicle) final; QList supportedMissionCommands(void) final; - bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) final; - QStringList flightModes (Vehicle* vehicle) final; - QString flightMode (uint8_t base_mode, uint32_t custom_mode) const final; - bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final; - void setGuidedMode (Vehicle* vehicle, bool guidedMode) final; - void pauseVehicle (Vehicle* vehicle) final; - void guidedModeRTL (Vehicle* vehicle) final; - void guidedModeLand (Vehicle* vehicle) final; - void guidedModeTakeoff (Vehicle* vehicle, double altitudeRel) final; - void guidedModeOrbit (Vehicle* vehicle, const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN) final; - void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; - void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeRel) final; - bool isGuidedMode (const Vehicle* vehicle) const; - int manualControlReservedButtonCount(void) final; - bool supportsManualControl (void) final; - void initializeVehicle (Vehicle* vehicle) final; - bool sendHomePositionToVehicle (void) final; - void addMetaDataToFact (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final; - QString getDefaultComponentIdParam (void) const final { return QString("SYS_AUTOSTART"); } - QString missionCommandOverrides (MAV_TYPE vehicleType) const final; - QString getVersionParam (void) final { return QString("SYS_PARAM_VER"); } - QString internalParameterMetaDataFile (void) final { return QString(":/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml"); } - void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } - QObject* loadParameterMetaData (const QString& metaDataFile); - bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message); + bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) final; + QStringList flightModes (Vehicle* vehicle) final; + QString flightMode (uint8_t base_mode, uint32_t custom_mode) const final; + bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final; + void setGuidedMode (Vehicle* vehicle, bool guidedMode) final; + void pauseVehicle (Vehicle* vehicle) final; + void guidedModeRTL (Vehicle* vehicle) final; + void guidedModeLand (Vehicle* vehicle) final; + void guidedModeTakeoff (Vehicle* vehicle, double altitudeRel) final; + void guidedModeOrbit (Vehicle* vehicle, const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN) final; + void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; + void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeRel) final; + bool isGuidedMode (const Vehicle* vehicle) const; + int manualControlReservedButtonCount(void) final; + bool supportsManualControl (void) final; + void initializeVehicle (Vehicle* vehicle) final; + bool sendHomePositionToVehicle (void) final; + void addMetaDataToFact (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final; + QString getDefaultComponentIdParam (void) const final { return QString("SYS_AUTOSTART"); } + QString missionCommandOverrides (MAV_TYPE vehicleType) const final; + QString getVersionParam (void) final { return QString("SYS_PARAM_VER"); } + QString internalParameterMetaDataFile (void) final { return QString(":/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml"); } + void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } + QObject* loadParameterMetaData (const QString& metaDataFile); + bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message); + GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) { return new PX4GeoFenceManager(vehicle); } // Use these constants to set flight modes using setFlightMode method. Don't use hardcoded string names since the // names may change. diff --git a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc new file mode 100644 index 000000000..e8f8891bc --- /dev/null +++ b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "PX4GeoFenceManager.h" +#include "Vehicle.h" +#include "FirmwarePlugin.h" +#include "ParameterLoader.h" + +PX4GeoFenceManager::PX4GeoFenceManager(Vehicle* vehicle) + : GeoFenceManager(vehicle) + , _firstParamLoadComplete(false) + , _circleRadiusFact(NULL) +{ + connect(_vehicle->getParameterLoader(), &ParameterLoader::parametersReady, this, &PX4GeoFenceManager::_parametersReady); +} + +PX4GeoFenceManager::~PX4GeoFenceManager() +{ + +} + +void PX4GeoFenceManager::_parametersReady(void) +{ + if (!_firstParamLoadComplete) { + _firstParamLoadComplete = true; + + _circleRadiusFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("GF_MAX_HOR_DIST")); + connect(_circleRadiusFact, &Fact::rawValueChanged, this, &PX4GeoFenceManager::_circleRadiusRawValueChanged); + emit circleRadiusChanged(circleRadius()); + + QStringList paramNames; + QStringList paramLabels; + + paramNames << QStringLiteral("GF_ACTION") << QStringLiteral("GF_MAX_HOR_DIST") << QStringLiteral("GF_MAX_VER_DIST"); + paramLabels << QStringLiteral("Action:") << QStringLiteral("Radius:") << QStringLiteral("Max Altitude:"); + + _params.clear(); + _paramLabels.clear(); + for (int i=0; iparameterExists(FactSystem::defaultComponentId, paramName)) { + Fact* paramFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, paramName); + _params << QVariant::fromValue(paramFact); + _paramLabels << paramLabels[i]; + } + } + emit paramsChanged(_params); + emit paramLabelsChanged(_paramLabels); + + emit circleSupportedChanged(circleSupported()); + + qCDebug(GeoFenceManagerLog) << "fenceSupported:circleSupported:polygonSupported:breachReturnSupported" << + fenceSupported() << circleSupported() << polygonSupported() << breachReturnSupported(); + } +} + +float PX4GeoFenceManager::circleRadius(void) const +{ + if (_circleRadiusFact) { + return _circleRadiusFact->rawValue().toFloat(); + } else { + return 0.0; + } +} + +void PX4GeoFenceManager::_circleRadiusRawValueChanged(QVariant value) +{ + emit circleRadiusChanged(value.toFloat()); + emit circleSupportedChanged(circleSupported()); +} + +bool PX4GeoFenceManager::circleSupported(void) const +{ + if (_circleRadiusFact) { + return _circleRadiusFact->rawValue().toFloat() >= 0.0; + } + + return false; +} diff --git a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h new file mode 100644 index 000000000..3101da89e --- /dev/null +++ b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#ifndef PX4GeoFenceManager_H +#define PX4GeoFenceManager_H + +#include "GeoFenceManager.h" +#include "QGCMAVLink.h" +#include "FactSystem.h" + +class PX4GeoFenceManager : public GeoFenceManager +{ + Q_OBJECT + +public: + PX4GeoFenceManager(Vehicle* vehicle); + ~PX4GeoFenceManager(); + + // Overrides from GeoFenceManager + bool fenceSupported (void) const final { return true; } + bool circleSupported (void) const final; + float circleRadius (void) const final; + QVariantList params (void) const final { return _params; } + QStringList paramLabels (void) const final { return _paramLabels; } + +private slots: + void _circleRadiusRawValueChanged(QVariant value); + void _parametersReady(void); + +private: + bool _firstParamLoadComplete; + + QVariantList _params; + QStringList _paramLabels; + + Fact* _circleRadiusFact; +}; + +#endif diff --git a/src/MissionEditor/GeoFenceEditor.qml b/src/MissionEditor/GeoFenceEditor.qml index f5fc9eef2..933ed48ae 100644 --- a/src/MissionEditor/GeoFenceEditor.qml +++ b/src/MissionEditor/GeoFenceEditor.qml @@ -44,7 +44,7 @@ QGCFlickable { anchors.margins: _margin anchors.left: parent.left anchors.top: parent.top - text: qsTr("Geo-Fence (WIP careful!)") + text: qsTr("GeoFence (WIP careful!)") color: "black" } diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 5989968e0..97bc41af9 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -471,16 +471,58 @@ QGCView { } } + // Mission/GeoFence selector + Item { + id: planElementSelector + anchors.topMargin: parent.height - ScreenTools.availableHeight + _margin + anchors.top: parent.top + anchors.leftMargin: parent.width - _rightPanelWidth + anchors.left: parent.left + width: planElementSelectorRow.width + height: geoFenceController.fenceSupported ? planElementSelectorRow.height : 0 + visible: geoFenceController.fenceSupported + + ExclusiveGroup { + id: planElementSelectorGroup + onCurrentChanged: { + var layerIsMission = current == planElementMission + _editingLayer = layerIsMission ? _layerMission : _layerGeoFence + _syncDropDownController = layerIsMission ? missionController : geoFenceController + } + } + + Row { + id: planElementSelectorRow + spacing: _horizontalMargin + + QGCRadioButton { + id: planElementMission + text: qsTr("Mission") + checked: true + exclusiveGroup: planElementSelectorGroup + color: mapPal.text + } + + QGCRadioButton { + id: planElementGeoFence + text: qsTr("GeoFence") + exclusiveGroup: planElementSelectorGroup + color: mapPal.text + } + } + } + // Mission Item Editor Item { - id: missionItemEditor - height: ScreenTools.availableHeight - anchors.bottom: parent.bottom - anchors.right: parent.right - width: _rightPanelWidth - opacity: _rightPanelOpacity - z: QGroundControl.zOrderTopMost - visible: _editingLayer == _layerMission + id: missionItemEditor + anchors.topMargin: _margin + anchors.top: planElementSelector.bottom + anchors.bottom: parent.bottom + anchors.right: parent.right + width: _rightPanelWidth + opacity: _rightPanelOpacity + z: QGroundControl.zOrderTopMost + visible: _editingLayer == _layerMission MouseArea { // This MouseArea prevents the Map below it from getting Mouse events. Without this @@ -528,8 +570,8 @@ QGCView { // GeoFence Editor Loader { - anchors.topMargin: parent.height - ScreenTools.availableHeight - anchors.top: parent.top + anchors.topMargin: _margin + anchors.top: planElementSelector.bottom anchors.right: parent.right opacity: _rightPanelOpacity z: QGroundControl.zOrderTopMost @@ -601,47 +643,6 @@ QGCView { spacing: ScreenTools.defaultFontPixelHeight z: QGroundControl.zOrderWidgets - DropButton { - id: layerButton - dropDirection: dropRight - viewportMargins: ScreenTools.defaultFontPixelWidth / 2 - exclusiveGroup: _dropButtonsExclusiveGroup - lightBorders: _lightWidgetBorders - visible: geoFenceController.fenceSupported - - dropDownComponent: Component { - Column { - spacing: ScreenTools.defaultFontPixelWidth * 0.5 - - QGCLabel { text: qsTr("Editing Layer:") } - - Row { - spacing: ScreenTools.defaultFontPixelWidth - - QGCButton { - text: qsTr("Mission") - - onClicked: { - layerButton.hideDropDown() - _editingLayer = _layerMission - _syncDropDownController = missionController - } - } - - QGCButton { - text: qsTr("GeoFence") - - onClicked: { - layerButton.hideDropDown() - _editingLayer = _layerGeoFence - _syncDropDownController = geoFenceController - } - } - } - } - } - } - RoundButton { id: addMissionItemsButton buttonImage: "/qmlimages/MapAddMission.svg" @@ -818,10 +819,10 @@ QGCView { id: syncLoadFromVehicleOverwrite QGCViewMessage { id: syncLoadFromVehicleCheck - message: qsTr("You have unsaved/unsent mission changes. Loading the mission from the Vehicle will lose these changes. Are you sure you want to load the mission from the Vehicle?") + message: qsTr("You have unsaved/unsent changes. Loading from the Vehicle will lose these changes. Are you sure you want to load from the Vehicle?") function accept() { hideDialog() - loadFromVehicle() + _syncDropDownController.loadFromVehicle() } } } @@ -830,10 +831,10 @@ QGCView { id: syncLoadFromFileOverwrite QGCViewMessage { id: syncLoadFromVehicleCheck - message: qsTr("You have unsaved/unsent mission changes. Loading a mission from a file will lose these changes. Are you sure you want to load a mission from a file?") + message: qsTr("You have unsaved/unsent changes. Loading a from a file will lose these changes. Are you sure you want to load from a file?") function accept() { hideDialog() - loadFromFile() + _syncDropDownController.loadFromSelectedFile() } } } @@ -841,10 +842,10 @@ QGCView { Component { id: removeAllPromptDialog QGCViewMessage { - message: qsTr("Are you sure you want to delete all mission items?") + message: qsTr("Are you sure you want to remove all items?") function accept() { itemDragger.clearItem() - missionController.removeAll() + _syncDropDownController.removeAll() hideDialog() } } @@ -857,11 +858,13 @@ QGCView { id: columnHolder spacing: _margin + property string _overwriteText: (_editingLayer == _layerMission) ? qsTr("Mission overwrite") : qsTr("GeoFence overwrite") + QGCLabel { width: sendSaveGrid.width wrapMode: Text.WordWrap text: _syncDropDownController.dirty ? - qsTr("You have unsaved changes to your mission. You should send to your vehicle, or save to a file:") : + qsTr("You have unsaved changes. You should send to your vehicle, or save to a file:") : qsTr("Sync:") } @@ -889,7 +892,7 @@ QGCView { onClicked: { syncButton.hideDropDown() if (_syncDropDownController.dirty) { - _root.showDialog(syncLoadFromVehicleOverwrite, qsTr("Mission overwrite"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel) + _root.showDialog(syncLoadFromVehicleOverwrite, columnHolder._overwriteText, _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel) } else { _syncDropDownController.loadFromVehicle() } @@ -913,7 +916,7 @@ QGCView { onClicked: { syncButton.hideDropDown() if (_syncDropDownController.dirty) { - _root.showDialog(syncLoadFromFileOverwrite, qsTr("Mission overwrite"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel) + _root.showDialog(syncLoadFromFileOverwrite, columnHolder._overwriteText, _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel) } else { _syncDropDownController.loadFromSelectedFile() } @@ -926,7 +929,7 @@ QGCView { onClicked: { syncButton.hideDropDown() _syncDropDownController.removeAll() - _root.showDialog(removeAllPromptDialog, qsTr("Delete all"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.No) + _root.showDialog(removeAllPromptDialog, qsTr("Remove all"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.No) } } } diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index 71bc2b46d..bdea38f36 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -83,8 +83,9 @@ void GeoFenceController::_activeVehicleSet(void) connect(geoFenceManager, &GeoFenceManager::paramsChanged, this, &GeoFenceController::paramsChanged); connect(geoFenceManager, &GeoFenceManager::paramLabelsChanged, this, &GeoFenceController::paramLabelsChanged); + _signalAll(); + if (_activeVehicle->getParameterLoader()->parametersAreReady()) { - _signalAll(); if (!syncInProgress()) { // We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle. // We don't request mission items for new vehicles since that will happen autamatically. @@ -223,7 +224,8 @@ void GeoFenceController::_setDirty(void) void GeoFenceController::_setPolygon(const QList& polygon) { _polygon.setPath(polygon); - setDirty(true); + // This is coming from a GeoFenceManager::loadFromVehicle call + setDirty(false); } float GeoFenceController::circleRadius(void) const diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 55c19e8d4..596228aad 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -188,7 +188,7 @@ void LinkManager::_addLink(LinkInterface* link) if (!(_mavlinkChannelsUsedBitMask & 1 << i)) { mavlink_reset_channel_status(i); link->_setMavlinkChannel(i); - _mavlinkChannelsUsedBitMask |= i << i; + _mavlinkChannelsUsedBitMask |= 1 << i; channelSet = true; break; } -- 2.22.0