Adding Orbit Mode to Guided Bar

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -183,6 +183,12 @@ void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
     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

@@ -109,6 +109,10 @@ public:
     ///     @param altitudeRel Relative altitude to takeoff to
     virtual void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel);
 
+    /// 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

@@ -269,6 +269,29 @@ void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
     vehicle->setFlightMode(landingFlightMode);
 }
 
+void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
+{
+    //-- If not in "guided" mode, make it so.
+    if(!isGuidedMode(vehicle))
+        setGuidedMode(vehicle, true);
+    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
+    mavlink_message_t msg;
+    mavlink_command_long_t cmd;
+    cmd.command = (uint16_t)MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE;
+    cmd.confirmation = 0;
+    cmd.param1 = radius;
+    cmd.param2 = velocity;
+    cmd.param3 = altitude;
+    cmd.param4 = NAN;
+    cmd.param5 = centerCoord.isValid() ? centerCoord.latitude()  : NAN;
+    cmd.param6 = centerCoord.isValid() ? centerCoord.longitude() : NAN;
+    cmd.param7 = centerCoord.isValid() ? centerCoord.altitude()  : NAN;
+    cmd.target_system = vehicle->id();
+    cmd.target_component = vehicle->defaultComponentId();
+    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
+    vehicle->sendMessageOnPriorityLink(msg);
+}
+
 void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
 {
     Q_UNUSED(altitudeRel);

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -39,6 +39,7 @@ public:
     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;

src/FlightDisplay/FlightDisplayViewWidgets.qml

@@ -279,10 +279,9 @@ Item {
         anchors.horizontalCenter:   parent.horizontalCenter
         width:                      guidedModeColumn.width  + (_margins * 2)
         height:                     guidedModeColumn.height + (_margins * 2)
-        radius:                     _margins
-        color:                      _lightWidgetBorders ? qgcPal.mapWidgetBorderLight : qgcPal.mapWidgetBorderDark
+        radius:                     ScreenTools.defaultFontPixelHeight * 0.25
+        color:                      _lightWidgetBorders ? Qt.rgba(qgcPal.mapWidgetBorderLight.r, qgcPal.mapWidgetBorderLight.g, qgcPal.mapWidgetBorderLight.b, 0.8) : Qt.rgba(qgcPal.mapWidgetBorderDark.r, qgcPal.mapWidgetBorderDark.g, qgcPal.mapWidgetBorderDark.b, 0.75)
         visible:                    _activeVehicle
-        opacity:                    0.9
         z:                          QGroundControl.zOrderWidgets
         state:                      "Shown"
 
@@ -340,6 +339,7 @@ Item {
         readonly property int confirmChangeAlt:     7
         readonly property int confirmGoTo:          8
         readonly property int confirmRetask:        9
+        readonly property int confirmOrbit:         10
 
         property int    confirmActionCode
         property real   _showMargin:    _margins
@@ -381,6 +381,12 @@ Item {
             case confirmRetask:
                 _activeVehicle.setCurrentMissionSequence(_flightMap._retaskSequence)
                 break;
+            case confirmOrbit:
+                //-- All parameters controlled by RC
+                _activeVehicle.guidedModeOrbit()
+                //-- Center on current flight map position and orbit with a 50m radius (velocity/direction controlled by the RC)
+                //_activeVehicle.guidedModeOrbit(QGroundControl.flightMapPosition, 50.0)
+                break;
             default:
                 console.warn(qsTr("Internal error: unknown confirmActionCode"), confirmActionCode)
             }
@@ -429,6 +435,9 @@ Item {
             case confirmRetask:
                 guidedModeConfirm.confirmText = qsTr("active waypoint change")
                 break;
+            case confirmOrbit:
+                guidedModeConfirm.confirmText = qsTr("enter orbit mode")
+                break;
             }
             _guidedModeBar.visible = false
             guidedModeConfirm.visible = true
@@ -488,6 +497,14 @@ Item {
                     visible:    (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed
                     onClicked:  _guidedModeBar.confirmAction(_guidedModeBar.confirmChangeAlt)
                 }
+
+                QGCButton {
+                    pointSize:  _guidedModeBar._fontPointSize
+                    text:       qsTr("Orbit")
+                    visible:    (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed
+                    onClicked:  _guidedModeBar.confirmAction(_guidedModeBar.confirmOrbit)
+                }
+
             } // Row
         } // Column
     } // Rectangle - Guided mode buttons

src/Vehicle/Vehicle.cc

@@ -1637,6 +1637,15 @@ void Vehicle::guidedModeChangeAltitude(double altitudeRel)
     _firmwarePlugin->guidedModeChangeAltitude(this, altitudeRel);
 }
 
+void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
+{
+    if (!guidedModeSupported()) {
+        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
+        return;
+    }
+    _firmwarePlugin->guidedModeOrbit(this, centerCoord, radius, velocity, altitude);
+}
+
 void Vehicle::pauseVehicle(void)
 {
     if (!pauseVehicleSupported()) {

src/Vehicle/Vehicle.h

@@ -349,6 +349,13 @@ public:
     /// Command vehicle to change to the specified relatice altitude
     Q_INVOKABLE void guidedModeChangeAltitude(double altitudeRel);
 
+    /// Command vehicle to orbit given center point
+    ///     @param centerCoord Center Coordinates
+    ///     @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);
+
     /// Command vehicle to pause at current location. If vehicle supports guide mode, vehicle will be left
     /// in guided mode after pause.
     Q_INVOKABLE void pauseVehicle(void);