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

@@ -57,7 +57,7 @@ public:
     ///     key:    firmware major version
     ///     value:  remapParamNameMinorVersionRemapMap_t entry
     typedef QMap<int, remapParamNameMinorVersionRemapMap_t> remapParamNameMajorVersionMap_t;
-    
+
     /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware.
     virtual void initializeVehicle(Vehicle* vehicle);
 
@@ -69,18 +69,18 @@ public:
     /// @return List of VehicleComponents for the specified vehicle. Caller owns returned objects and must
     ///         free when no longer needed.
     virtual QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle);
-    
+
     /// Returns the list of available flight modes
     virtual QStringList flightModes(Vehicle* vehicle) {
-		Q_UNUSED(vehicle);
+        Q_UNUSED(vehicle);
-		return QStringList();
+        return QStringList();
-	}
+    }
-    
+
     /// Returns the name for this flight mode. Flight mode names must be human readable as well as audio speakable.
     ///     @param base_mode Base mode from mavlink HEARTBEAT message
     ///     @param custom_mode Custom mode from mavlink HEARTBEAT message
     virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode) const;
-    
+
     /// Sets base_mode and custom_mode to specified flight mode.
     ///     @param[out] base_mode Base mode for SET_MODE mavlink message
     ///     @param[out] custom_mode Custom mode for SET_MODE mavlink message
@@ -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);
 
@@ -122,7 +126,7 @@ public:
     /// The remainder can be assigned to Vehicle actions.
     /// @return -1: reserver all buttons, >0 number of buttons to reserve
     virtual int manualControlReservedButtonCount(void);
-    
+
     /// Called before any mavlink message is processed by Vehicle such that the firmwre plugin
     /// can adjust any message characteristics. This is handy to adjust or differences in mavlink
     /// spec implementations such that the base code can remain mavlink generic.
@@ -130,7 +134,7 @@ public:
     ///     @param message[in,out] Mavlink message to adjust if needed.
     /// @return false: skip message, true: process message
     virtual bool adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
-    
+
     /// Called before any mavlink message is sent to the Vehicle so plugin can adjust any message characteristics.
     /// This is handy to adjust or differences in mavlink spec implementations such that the base code can remain
     /// mavlink generic.
@@ -145,7 +149,7 @@ public:
     ///             it, it may or may not return a home position back in position 0.
     ///     false: Do not send first item to vehicle, sequence numbers must be adjusted
     virtual bool sendHomePositionToVehicle(void);
-    
+
     /// Returns the parameter that is used to identify the default component
     virtual QString getDefaultComponentIdParam(void) const { return QString(); }
 

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

@@ -34,14 +34,15 @@ public:
     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        setGuidedMode                   (Vehicle* vehicle, bool guidedMode) final;
-    void        pauseVehicle(Vehicle* vehicle) final;
+    void        pauseVehicle                    (Vehicle* vehicle) final;
-    void        guidedModeRTL(Vehicle* vehicle) final;
+    void        guidedModeRTL                   (Vehicle* vehicle) final;
-    void        guidedModeLand(Vehicle* vehicle) final;
+    void        guidedModeLand                  (Vehicle* vehicle) final;
-    void        guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) final;
+    void        guidedModeTakeoff               (Vehicle* vehicle, double altitudeRel) final;
-    void        guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
+    void        guidedModeOrbit                 (Vehicle* vehicle, const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN) final;
-    void        guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final;
+    void        guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
-    bool        isGuidedMode(const Vehicle* vehicle) const;
+    void        guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeRel) final;
+    bool        isGuidedMode                    (const Vehicle* vehicle) const;
     int         manualControlReservedButtonCount(void) final;
     void        initializeVehicle               (Vehicle* vehicle) final;
     bool        sendHomePositionToVehicle       (void) final;

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
+        radius:                     ScreenTools.defaultFontPixelHeight * 0.25
-        color:                      _lightWidgetBorders ? qgcPal.mapWidgetBorderLight : qgcPal.mapWidgetBorderDark
+        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);