diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 344748ac96bf8aac2659006ad708564f3f43d3ed..70be22b1a421977be42a8f214cc085a08d32e131 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -270,12 +270,6 @@ void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel) qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); } -void FirmwarePlugin::guidedModeOrbit(Vehicle* /*vehicle*/, const QGeoCoordinate& /*centerCoord*/, double /*radius*/, double /*velocity*/, double /*altitude*/) -{ - // Not supported by generic vehicle - qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); -} - void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) { // Not supported by generic vehicle diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index c161e096c5b3d685143d7a23b3a04f9ff2a4d646..22020443aa4ca4c94958d3a50254adbad3858b2b 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -136,10 +136,6 @@ public: /// Command the vehicle to start the mission virtual void startMission(Vehicle* vehicle); - /// Command vehicle to orbit given center point - /// @param centerCoord Center Coordinates - virtual void guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude); - /// Command vehicle to move to specified location (altitude is included and relative) virtual void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 100b3f26dd9ef764a007073354a5e1489320c6f7..2cb1c3f0bc63257c4e0ec2afbc75b10fe57427d7 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -232,7 +232,7 @@ bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities c { int available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability; if (vehicle->multiRotor() || vehicle->vtol()) { - available |= TakeoffVehicleCapability; + available |= TakeoffVehicleCapability | OrbitModeCapability; } return (capabilities & available) == capabilities; @@ -364,24 +364,6 @@ void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle) _setFlightModeAndValidate(vehicle, _landingFlightMode); } -void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude) -{ - if (!isGuidedMode(vehicle)) { - setGuidedMode(vehicle, true); - } - - vehicle->sendMavCommand(vehicle->defaultComponentId(), - MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE, - true, // show error if fails - radius, - velocity, - altitude, - NAN, - centerCoord.isValid() ? centerCoord.latitude() : NAN, - centerCoord.isValid() ? centerCoord.longitude() : NAN, - centerCoord.isValid() ? centerCoord.altitude() : NAN); -} - void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle) { Q_UNUSED(vehicleId); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index ddc69af0df0ef9292f603fcf143cfd51c895acfd..f4118c260aa28f05280201723afcdc01c20b17e3 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -46,7 +46,6 @@ public: void guidedModeRTL (Vehicle* vehicle) override; void guidedModeLand (Vehicle* vehicle) override; void guidedModeTakeoff (Vehicle* vehicle, double takeoffAltRel) override; - void guidedModeOrbit (Vehicle* vehicle, const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN) override; void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override; void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeRel) override; double minimumTakeoffAltitude (Vehicle* vehicle) override; diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index d9f3c3217c6582e4dc80fc3af09018f1fb14c135..1eb9e52b9cefbfa02e88bdf6ab7c1e3df8557599 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -508,11 +508,10 @@ QGCView { z: _panel.z + 4 title: qsTr("Fly") maxHeight: (_flightVideo.visible ? _flightVideo.y : parent.height) - toolStrip.y - buttonVisible: [ _useChecklist, _guidedController.showTakeoff || !_guidedController.showLand, _guidedController.showLand && !_guidedController.showTakeoff, true, true, true, _guidedController.smartShotsAvailable ] - buttonEnabled: [ _useChecklist && _activeVehicle, _guidedController.showTakeoff, _guidedController.showLand, _guidedController.showRTL, _guidedController.showPause, _anyActionAvailable, _anySmartShotAvailable ] + buttonVisible: [ _useChecklist, _guidedController.showTakeoff || !_guidedController.showLand, _guidedController.showLand && !_guidedController.showTakeoff, true, true, true ] + buttonEnabled: [ _useChecklist && _activeVehicle, _guidedController.showTakeoff, _guidedController.showLand, _guidedController.showRTL, _guidedController.showPause, _anyActionAvailable ] property bool _anyActionAvailable: _guidedController.showStartMission || _guidedController.showResumeMission || _guidedController.showChangeAlt || _guidedController.showLandAbort - property bool _anySmartShotAvailable: _guidedController.showOrbit property var _actionModel: [ { title: _guidedController.startMissionTitle, @@ -545,14 +544,6 @@ QGCView { visible: _guidedController.showLandAbort } ] - property var _smartShotModel: [ - { - title: _guidedController.orbitTitle, - text: _guidedController.orbitMessage, - action: _guidedController.actionOrbit, - visible: _guidedController.showOrbit - } - ] model: [ { @@ -584,28 +575,15 @@ QGCView { name: qsTr("Action"), iconSource: "/res/action.svg", action: -1 - }, - /* - No firmware support any smart shots yet - { - name: qsTr("Smart"), - iconSource: "/qmlimages/MapCenter.svg", - action: -1 - }, - */ + } ] onClicked: { guidedActionsController.closeAll() var action = model[index].action if (action === -1) { - if (index == 5) { - guidedActionList.model = _actionModel - guidedActionList.visible = true - } else if (index == 6) { - guidedActionList.model = _smartShotModel - guidedActionList.visible = true - } + guidedActionList.model = _actionModel + guidedActionList.visible = true } else { _guidedController.confirmAction(action) } diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index bbc199045103f40b1b2dcf797804e61d7f673b0a..134f10dd81e3311ab94bd960146d745b97a13dcf 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -49,7 +49,7 @@ FlightMap { property var _rallyPointController: _planMasterController.rallyPointController property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property var _activeVehicleCoordinate: _activeVehicle ? _activeVehicle.coordinate : QtPositioning.coordinate() - property var _gotoHereCoordinate: QtPositioning.coordinate() + property var _guidedLocationCoordinate: QtPositioning.coordinate() property real _toolButtonTopMargin: parent.height - ScreenTools.availableHeight + (ScreenTools.defaultFontPixelHeight / 2) property bool _disableVehicleTracking: false @@ -264,39 +264,74 @@ FlightMap { } } - // GoTo here waypoint + // Camera trigger points + MapItemView { + model: _activeVehicle ? _activeVehicle.cameraTriggerPoints : 0 + + delegate: CameraTriggerIndicator { + coordinate: object.coordinate + z: QGroundControl.zOrderTopMost + } + } + + // Guided action location MapQuickItem { - coordinate: _gotoHereCoordinate - visible: _activeVehicle && _activeVehicle.guidedModeSupported && _gotoHereCoordinate.isValid + id: guidedLocationItem + coordinate: _guidedLocationCoordinate + visible: _activeVehicle && _activeVehicle.guidedModeSupported && _guidedLocationCoordinate.isValid z: QGroundControl.zOrderMapItems anchorPoint.x: sourceItem.anchorPointX anchorPoint.y: sourceItem.anchorPointY + property bool gotoLocation: true ///< true: Used for go to location, false: used for orbit + sourceItem: MissionItemIndexLabel { checked: true index: -1 - label: qsTr("Goto here", "Goto here waypoint") + label: guidedLocationItem.gotoLocation ? qsTr("Goto here", "Goto here waypoint") : qsTr("Orbit here", "Orbit here waypoint") } } - // Camera trigger points - MapItemView { - model: _activeVehicle ? _activeVehicle.cameraTriggerPoints : 0 - - delegate: CameraTriggerIndicator { - coordinate: object.coordinate - z: QGroundControl.zOrderTopMost - } - } - // Handle guided mode clicks MouseArea { anchors.fill: parent + Menu { + id: clickMenu + + MenuItem { + text: qsTr("Go to location") + visible: guidedActionsController.showGotoLocation + + onTriggered: { + guidedLocationItem.gotoLocation = true + guidedActionsController.confirmAction(guidedActionsController.actionGoto, _guidedLocationCoordinate) + } + } + + MenuItem { + text: qsTr("Orbit at location") + visible: guidedActionsController.showOrbit + onTriggered: { + guidedLocationItem.gotoLocation = false + guidedActionsController.confirmAction(guidedActionsController.actionOrbit, _guidedLocationCoordinate) + } + } + } + onClicked: { - if (guidedActionsController.showGotoLocation && !guidedActionsController.guidedUIVisible) { - _gotoHereCoordinate = flightMap.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */) - guidedActionsController.confirmAction(guidedActionsController.actionGoto, _gotoHereCoordinate) + if (guidedActionsController.guidedUIVisible || (!guidedActionsController.showGotoLocation && !guidedActionsController.showOrbit)) { + return + } + _guidedLocationCoordinate = flightMap.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */) + if (guidedActionsController.showGotoLocation && guidedActionsController.showOrbit) { + clickMenu.popup() + } else if (guidedActionsController.showGotoLocation) { + guidedLocationItem.gotoLocation = true + guidedActionsController.confirmAction(guidedActionsController.actionGoto, _guidedLocationCoordinate) + } else if (guidedActionsController.showOrbit) { + guidedLocationItem.gotoLocation = false + guidedActionsController.confirmAction(guidedActionsController.actionOrbit, _guidedLocationCoordinate) } } } diff --git a/src/FlightDisplay/GuidedActionConfirm.qml b/src/FlightDisplay/GuidedActionConfirm.qml index 971fa460d9b375989568df84e79bf0623b29539e..8d36d74dc1a6688ef0e13d1164add68302237ec9 100644 --- a/src/FlightDisplay/GuidedActionConfirm.qml +++ b/src/FlightDisplay/GuidedActionConfirm.qml @@ -100,12 +100,13 @@ Rectangle { onAccept: { _root.visible = false + var altitudeChange = 0 if (altitudeSlider.visible) { - _root.actionData = altitudeSlider.getValue() + altitudeChange = altitudeSlider.getAltitudeChangeValue() altitudeSlider.visible = false } hideTrigger = false - guidedController.executeAction(_root.action, _root.actionData) + guidedController.executeAction(_root.action, _root.actionData, altitudeChange) } onReject: { diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index caac1e7b2bd3e5b52e5084d221d0342d5f58f7ff..27b7b7b0c21edcfdd67b0b846e523d9ec9b5a718 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -62,9 +62,9 @@ Item { readonly property string landMessage: qsTr("Land the vehicle at the current position.") readonly property string rtlMessage: qsTr("Return to the home position of the vehicle.") readonly property string changeAltMessage: qsTr("Change the altitude of the vehicle up or down.") - readonly property string gotoMessage: qsTr("Move the vehicle to the location clicked on the map.") + readonly property string gotoMessage: qsTr("Move the vehicle to the specified location.") property string setWaypointMessage: qsTr("Adjust current waypoint to %1.").arg(_actionData) - readonly property string orbitMessage: qsTr("Orbit the vehicle around the current location.") + readonly property string orbitMessage: qsTr("Orbit the vehicle around the specified location. Warning: WORK IN PROGRESS!") readonly property string landAbortMessage: qsTr("Abort the landing sequence.") readonly property string pauseMessage: qsTr("Pause the vehicle at it's current position, adjusting altitude up or down as needed.") readonly property string mvPauseMessage: qsTr("Pause all vehicles at their current position.") @@ -103,7 +103,7 @@ Item { property bool showContinueMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && _vehicleFlying && (_currentMissionIndex < missionController.visualItems.count - 1) property bool showPause: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.pauseVehicleSupported && _vehicleFlying && !_vehiclePaused property bool showChangeAlt: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive - property bool showOrbit: _guidedActionsEnabled && !_hideOrbit && _vehicleFlying && _activeVehicle.orbitModeSupported && _vehicleArmed && !_missionActive + property bool showOrbit: _guidedActionsEnabled && !_hideOrbit && _vehicleFlying && _activeVehicle.orbitModeSupported && !_missionActive property bool showLandAbort: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.fixedWing && _vehicleLanding property bool showGotoLocation: _guidedActionsEnabled && _vehicleFlying @@ -190,7 +190,8 @@ Item { _vehicleWasFlying = true } } - property var _actionData + + property var _actionData on_FlightModeChanged: { _vehiclePaused = _activeVehicle ? _flightMode === _activeVehicle.pauseFlightMode : false @@ -299,6 +300,8 @@ Item { confirmDialog.title = orbitTitle confirmDialog.message = orbitMessage confirmDialog.hideTrigger = Qt.binding(function() { return !showOrbit }) + altitudeSlider.reset() + altitudeSlider.visible = true break; case actionLandAbort: confirmDialog.title = landAbortTitle @@ -335,7 +338,7 @@ Item { } // Executes the specified action - function executeAction(actionCode, actionData) { + function executeAction(actionCode, actionData, actionAltitudeChange) { var i; var rgVehicle; switch (actionCode) { @@ -346,7 +349,7 @@ Item { _activeVehicle.guidedModeLand() break case actionTakeoff: - _activeVehicle.guidedModeTakeoff(actionData) + _activeVehicle.guidedModeTakeoff(actionAltitudeChange) break case actionResumeMission: case actionResumeMissionUploadFail: @@ -376,7 +379,7 @@ Item { _activeVehicle.emergencyStop() break case actionChangeAlt: - _activeVehicle.guidedModeChangeAltitude(actionData) + _activeVehicle.guidedModeChangeAltitude(actionAltitudeChange) break case actionGoto: _activeVehicle.guidedModeGotoLocation(actionData) @@ -385,14 +388,14 @@ Item { _activeVehicle.setCurrentMissionSequence(actionData) break case actionOrbit: - _activeVehicle.guidedModeOrbit() + _activeVehicle.guidedModeOrbit(actionData, 10 /* Hacked fixed radius */, _activeVehicle.altitudeAMSL + actionAltitudeChange) break case actionLandAbort: _activeVehicle.abortLanding(50) // hardcoded value for climbOutAltitude that is currently ignored break case actionPause: _activeVehicle.pauseVehicle() - _activeVehicle.guidedModeChangeAltitude(actionData) + _activeVehicle.guidedModeChangeAltitude(actionAltitudeChange) break case actionMVPause: rgVehicle = QGroundControl.multiVehicleManager.vehicles diff --git a/src/FlightDisplay/GuidedAltitudeSlider.qml b/src/FlightDisplay/GuidedAltitudeSlider.qml index 2144711d93392d98700045ca28b2e50b3858966e..a4e61d726ff8c369885142d04f2f2faaa15a078a 100644 --- a/src/FlightDisplay/GuidedAltitudeSlider.qml +++ b/src/FlightDisplay/GuidedAltitudeSlider.qml @@ -36,7 +36,8 @@ Rectangle { altField.setToMinimumTakeoff() } - function getValue() { + /// Returns the user specified change in altitude from the current vehicle altitude + function getAltitudeChangeValue() { return altField.newAltitudeMeters - _vehicleAltitude } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f81769ee4fafb4cb314446dcc5a7a48ad6635b2b..eb08ed71883fa0b1ebb6f2fde349db3e38944a9b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2751,13 +2751,30 @@ void Vehicle::guidedModeChangeAltitude(double altitudeChange) _firmwarePlugin->guidedModeChangeAltitude(this, altitudeChange); } -void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude) +void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double amslAltitude) { if (!orbitModeSupported()) { qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle.")); return; } - _firmwarePlugin->guidedModeOrbit(this, centerCoord, radius, velocity, altitude); + + double lat, lon, alt; + if (centerCoord.isValid()) { + lat = lon = alt = qQNaN(); + } else { + lat = centerCoord.latitude(); + lon = centerCoord.longitude(); + alt = amslAltitude; + } + + sendMavCommand(defaultComponentId(), + MAV_CMD_DO_ORBIT, + true, // show error if fails + radius, + qQNaN(), // Use default velocity + 0, // Vehicle points to center + qQNaN(), // reserved + lat, lon, alt); } void Vehicle::pauseVehicle(void) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index e2b8a646bb7bf6f02e4134be03e100aa0b3e298e..03a8c520f033180b0c51e60d1478b210655a94e1 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -582,11 +582,10 @@ public: Q_INVOKABLE void guidedModeChangeAltitude(double altitudeChange); /// Command vehicle to orbit given center point - /// @param centerCoord Center Coordinates + /// @param centerCoord Orit around this point /// @param radius Distance from vehicle to centerCoord - /// @param velocity Orbit velocity (positive CW, negative CCW) - /// @param altitude Desired Vehicle Altitude - Q_INVOKABLE void guidedModeOrbit(const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN); + /// @param amslAltitude Desired vehicle altitude + Q_INVOKABLE void guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double amslAltitude); /// Command vehicle to pause at current location. If vehicle supports guide mode, vehicle will be left /// in guided mode after pause.