diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 69a153ccd244ac33a72e05c37101f24fd40fd9c3..bac34679a75151a1e54e6f5e002e7603784a2a3f 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -84,6 +84,7 @@ public: QString flightMode (uint8_t base_mode, uint32_t custom_mode) const override; bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) override; bool isGuidedMode (const Vehicle* vehicle) const override; + QString gotoFlightMode (void) const override { return QStringLiteral("Guided"); } QString rtlFlightMode (void) const override { return QString("RTL"); } QString missionFlightMode (void) const override { return QString("Auto"); } void pauseVehicle (Vehicle* vehicle) override; diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 057aca88f1604da82e3a5e34daa7bd08a7641b8e..db2a86969394f52d3033e41e4e42a1e4d2c43960 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -757,3 +757,8 @@ int FirmwarePlugin::versionCompare(Vehicle* vehicle, QString& compare) int patch = versionNumbers[2].toInt(); return versionCompare(vehicle, major, minor, patch); } + +QString FirmwarePlugin::gotoFlightMode(void) const +{ + return QString(); +} diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index d5c442b164a0fa21cafb74fd7c6842a16512c3ca..81d2d2abcd7e0e9a4fc87b1142ae2756b4c70a6b 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -114,6 +114,9 @@ public: /// Returns whether the vehicle is in guided mode or not. virtual bool isGuidedMode(const Vehicle* vehicle) const; + /// Returns the flight mode which the vehicle will be in if it is performing a goto location + virtual QString gotoFlightMode(void) const; + /// Set guided flight mode virtual void setGuidedMode(Vehicle* vehicle, bool guidedMode); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index fa2a4e8796a7fb35687396ec533aa2e75bbe2e2a..2c8059b61361edacda90a4d1d17947de254c05d3 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -42,6 +42,7 @@ public: QString rtlFlightMode (void) const override { return _rtlFlightMode; } QString landFlightMode (void) const override { return _landingFlightMode; } QString takeControlFlightMode (void) const override { return _manualFlightMode; } + QString gotoFlightMode (void) const override { return _holdFlightMode; } void pauseVehicle (Vehicle* vehicle) override; void guidedModeRTL (Vehicle* vehicle) override; void guidedModeLand (Vehicle* vehicle) override; diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 788c2f21273ee2e2606d179ebec1434e419fcc8f..ce8a6eaf72a965739f22a9131934921f7a22c5d7 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -295,6 +295,7 @@ FlightMap { } } + // GoTo Location visuals MapQuickItem { id: gotoLocationItem visible: false @@ -308,6 +309,22 @@ FlightMap { label: qsTr("Goto here", "Goto here waypoint") } + property bool inGotoFlightMode: _activeVehicle ? _activeVehicle.flightMode === _activeVehicle.gotoFlightMode : false + property var activeVehicle: _activeVehicle + + onInGotoFlightModeChanged: { + if (!inGotoFlightMode && visible) { + // Hide goto indicator when vehicle falls out of guided mode + visible = false + } + } + + onActiveVehicleChanged: { + if (!_activeVehicle) { + visible = false + } + } + function show(coord) { gotoLocationItem.coordinate = coord gotoLocationItem.visible = true @@ -316,10 +333,17 @@ FlightMap { function hide() { gotoLocationItem.visible = false } - } - // Orbit visuals + function actionConfirmed() { + // We leave the indicator visible. The handling for onInGuidedModeChanged will hide it. + } + + function actionCancelled() { + hide() + } + } + // Orbit editing visuals QGCMapCircleVisuals { id: orbitMapCircle mapControl: parent @@ -328,9 +352,16 @@ FlightMap { property alias center: _mapCircle.center property alias clockwiseRotation: _mapCircle.clockwiseRotation + property var activeVehicle: _activeVehicle readonly property real defaultRadius: 30 + onActiveVehicleChanged: { + if (!_activeVehicle) { + visible = false + } + } + function show(coord) { _mapCircle.radius.rawValue = defaultRadius orbitMapCircle.center = coord @@ -341,6 +372,15 @@ FlightMap { orbitMapCircle.visible = false } + function actionConfirmed() { + // Live orbit status is handled by telemetry so we hide here and telemetry will show again. + hide() + } + + function actionCancelled() { + hide() + } + function radius() { return _mapCircle.radius.rawValue } @@ -357,7 +397,6 @@ FlightMap { } // Orbit telemetry visuals - QGCMapCircleVisuals { id: orbitTelemetryCircle mapControl: parent @@ -395,7 +434,7 @@ FlightMap { onTriggered: { gotoLocationItem.show(clickMenu.coord) orbitMapCircle.hide() - guidedActionsController.confirmAction(guidedActionsController.actionGoto, clickMenu.coord) + guidedActionsController.confirmAction(guidedActionsController.actionGoto, clickMenu.coord, gotoLocationItem) } } @@ -406,7 +445,7 @@ FlightMap { onTriggered: { orbitMapCircle.show(clickMenu.coord) gotoLocationItem.hide() - guidedActionsController.confirmAction(guidedActionsController.actionOrbit, clickMenu.coord) + guidedActionsController.confirmAction(guidedActionsController.actionOrbit, clickMenu.coord, orbitMapCircle) } } } diff --git a/src/FlightDisplay/GuidedActionConfirm.qml b/src/FlightDisplay/GuidedActionConfirm.qml index 8d36d74dc1a6688ef0e13d1164add68302237ec9..177e1944ad82a013cb151b143173a44f30027e75 100644 --- a/src/FlightDisplay/GuidedActionConfirm.qml +++ b/src/FlightDisplay/GuidedActionConfirm.qml @@ -34,16 +34,14 @@ Rectangle { property int action property var actionData property bool hideTrigger: false + property var mapIndicator property real _margins: ScreenTools.defaultFontPixelWidth property bool _emergencyAction: action === guidedController.actionEmergencyStop onHideTriggerChanged: { if (hideTrigger) { - hideTrigger = false - altitudeSlider.visible = false - visibleTimer.stop() - visible = false + confirmCancelled() } } @@ -57,6 +55,17 @@ Rectangle { } } + function confirmCancelled() { + altitudeSlider.visible = false + visible = false + hideTrigger = false + visibleTimer.stop() + if (mapIndicator) { + mapIndicator.actionCancelled() + mapIndicator = undefined + } + } + Timer { id: visibleTimer interval: 1000 @@ -107,12 +116,10 @@ Rectangle { } hideTrigger = false guidedController.executeAction(_root.action, _root.actionData, altitudeChange) - } - - onReject: { - altitudeSlider.visible = false - _root.visible = false - hideTrigger = false + if (mapIndicator) { + mapIndicator.actionConfirmed() + mapIndicator = undefined + } } } } @@ -127,12 +134,10 @@ Rectangle { source: "/res/XDelete.svg" fillMode: Image.PreserveAspectFit color: qgcPal.text + QGCMouseArea { fillItem: parent - onClicked: { - altitudeSlider.visible = false - _root.visible = false - } + onClicked: confirmCancelled() } } } diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 58b3b68cfc4940d94c595079ceefe2cd70445f00..c099ed669c21dbabb532d645bce9cd9e7bf2e303 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -209,12 +209,13 @@ Item { } // Called when an action is about to be executed in order to confirm - function confirmAction(actionCode, actionData) { + function confirmAction(actionCode, actionData, mapIndicator) { var showImmediate = true closeAll() confirmDialog.action = actionCode confirmDialog.actionData = actionData confirmDialog.hideTrigger = true + confirmDialog.mapIndicator = mapIndicator _actionData = actionData switch (actionCode) { case actionArm: @@ -385,7 +386,6 @@ Item { break case actionOrbit: _activeVehicle.guidedModeOrbit(orbitMapCircle.center, orbitMapCircle.radius() * (orbitMapCircle.clockwiseRotation ? 1 : -1), _activeVehicle.altitudeAMSL.rawValue + actionAltitudeChange) - orbitMapCircle.hide() break case actionLandAbort: _activeVehicle.abortLanding(50) // hardcoded value for climbOutAltitude that is currently ignored diff --git a/src/QmlControls/SliderSwitch.qml b/src/QmlControls/SliderSwitch.qml index 2fcf738dc6c88df66e0d67bba9f29b1e01ee10f3..112223a47d6d63c9a72307d855ca7d4d0bb54d7e 100644 --- a/src/QmlControls/SliderSwitch.qml +++ b/src/QmlControls/SliderSwitch.qml @@ -14,7 +14,6 @@ Rectangle { color: qgcPal.windowShade signal accept ///< Action confirmed - signal reject ///< Action rejected property string confirmText ///< Text for slider property alias fontPointSize: label.font.pointSize ///< Point size for text @@ -70,12 +69,6 @@ Rectangle { property bool dragActive: drag.active property real _dragOffset: 1 - //Component.onCompleted: console.log(height, ScreenTools.minTouchPixels) - - onPressed: { - mouse.x - } - onDragActiveChanged: { if (!sliderDragArea.drag.active) { if (slider.x > _maxXDrag - _border) { diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f8cd2b75dcc4d1e2b2c5dc77c488bda6486e5cd3..4b9b33ef6e71b62d3c19c916f666fd761cffff64 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2962,6 +2962,11 @@ bool Vehicle::takeoffVehicleSupported() const return _firmwarePlugin->isCapable(this, FirmwarePlugin::TakeoffVehicleCapability); } +QString Vehicle::gotoFlightMode() const +{ + return _firmwarePlugin->gotoFlightMode(); +} + void Vehicle::guidedModeRTL(void) { if (!guidedModeSupported()) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 4afdb3b6cb3f8267a9cf97caafa13d4dd36cedf3..b330984442289aba2681bde17bca3ad7ae07baf1 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -636,13 +636,14 @@ public: Q_PROPERTY(QGCMapCircle* orbitMapCircle READ orbitMapCircle CONSTANT) // Vehicle state used for guided control - Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying - Q_PROPERTY(bool landing READ landing NOTIFY landingChanged) ///< Vehicle is in landing pattern (DO_LAND_START) - Q_PROPERTY(bool guidedMode READ guidedMode WRITE setGuidedMode NOTIFY guidedModeChanged) ///< Vehicle is in Guided mode and can respond to guided commands - Q_PROPERTY(bool guidedModeSupported READ guidedModeSupported CONSTANT) ///< Guided mode commands are supported by this vehicle - Q_PROPERTY(bool pauseVehicleSupported READ pauseVehicleSupported CONSTANT) ///< Pause vehicle command is supported - Q_PROPERTY(bool orbitModeSupported READ orbitModeSupported CONSTANT) ///< Orbit mode is supported by this vehicle - Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT) ///< Guided takeoff supported + Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying + Q_PROPERTY(bool landing READ landing NOTIFY landingChanged) ///< Vehicle is in landing pattern (DO_LAND_START) + Q_PROPERTY(bool guidedMode READ guidedMode WRITE setGuidedMode NOTIFY guidedModeChanged) ///< Vehicle is in Guided mode and can respond to guided commands + Q_PROPERTY(bool guidedModeSupported READ guidedModeSupported CONSTANT) ///< Guided mode commands are supported by this vehicle + Q_PROPERTY(bool pauseVehicleSupported READ pauseVehicleSupported CONSTANT) ///< Pause vehicle command is supported + Q_PROPERTY(bool orbitModeSupported READ orbitModeSupported CONSTANT) ///< Orbit mode is supported by this vehicle + Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT) ///< Guided takeoff supported + Q_PROPERTY(QString gotoFlightMode READ gotoFlightMode CONSTANT) ///< Flight mode vehicle is in while performing goto Q_PROPERTY(ParameterManager* parameterManager READ parameterManager CONSTANT) @@ -761,10 +762,11 @@ public: /// @param percent 0-no power, 100-full power Q_INVOKABLE void motorTest(int motor, int percent); - bool guidedModeSupported (void) const; - bool pauseVehicleSupported (void) const; - bool orbitModeSupported (void) const; - bool takeoffVehicleSupported(void) const; + bool guidedModeSupported (void) const; + bool pauseVehicleSupported (void) const; + bool orbitModeSupported (void) const; + bool takeoffVehicleSupported (void) const; + QString gotoFlightMode (void) const; // Property accessors