Mavlink Orbit support

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

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);
 

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);

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;

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
-                    }
                 } else {
                     _guidedController.confirmAction(action)
                 }

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
+    // Handle guided mode clicks
+    MouseArea {
+        anchors.fill: parent
 
-        delegate: CameraTriggerIndicator {
-            coordinate:     object.coordinate
-            z:              QGroundControl.zOrderTopMost
+        Menu {
+            id: clickMenu
+
+            MenuItem {
+                text:           qsTr("Go to location")
+                visible:        guidedActionsController.showGotoLocation
+
+                onTriggered: {
+                    guidedLocationItem.gotoLocation = true
+                    guidedActionsController.confirmAction(guidedActionsController.actionGoto, _guidedLocationCoordinate)
                 }
             }
 
-    // Handle guided mode clicks
-    MouseArea {
-        anchors.fill: parent
+            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)
             }
         }
     }

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: {

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,6 +190,7 @@ Item {
             _vehicleWasFlying = true
         }
     }
+
     property var _actionData
 
     on_FlightModeChanged: {
@@ -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

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
     }
 

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)

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.