Merge pull request #3804 from dogmaphobic/orbitOnGuidedBar

Adding Orbit Mode to Guided Bar

src/FactSystem/ParameterLoader.cc

@@ -773,7 +773,7 @@ void ParameterLoader::_saveToEEPROM(void)
 {
     if (_saveRequired) {
         _saveRequired = false;
-        if (_vehicle->firmwarePlugin()->isCapable(FirmwarePlugin::MavCmdPreflightStorageCapability)) {
+        if (_vehicle->firmwarePlugin()->isCapable(_vehicle, FirmwarePlugin::MavCmdPreflightStorageCapability)) {
             mavlink_message_t msg;
             mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0);
             _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -143,7 +143,7 @@ APMFirmwarePlugin::APMFirmwarePlugin(void)
 
 }
 
-bool APMFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
+bool APMFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities)
 {
     return (capabilities & (SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
 }

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -73,7 +73,7 @@ public:
     QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle) final;
     QList<MAV_CMD> supportedMissionCommands(void) final;
 
-    bool        isCapable(FirmwareCapabilities capabilities);
+    bool        isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities);
     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;

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -98,7 +98,7 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe
     return majorVersionNumber == 3 ? 4: Vehicle::versionNotSetValue;
 }
 
-bool ArduCopterFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
+bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities)
 {
     return (capabilities & (SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability)) == capabilities;
 }

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h

@@ -53,7 +53,7 @@ public:
     ArduCopterFirmwarePlugin(void);
 
     // Overrides from FirmwarePlugin
-    bool isCapable(FirmwareCapabilities capabilities) final;
+    bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
     bool isPaused(const Vehicle* vehicle) const final;
     void setGuidedMode(Vehicle* vehicle, bool guidedMode) final;
     void pauseVehicle(Vehicle* vehicle) final;

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -15,8 +15,9 @@
 
 const char* guided_mode_not_supported_by_vehicle = "Guided mode not supported by Vehicle.";
 
-bool FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
+bool FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities)
 {
+    Q_UNUSED(vehicle);
     Q_UNUSED(capabilities);
     return false;
 }
@@ -183,6 +184,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

@@ -40,7 +40,8 @@ public:
         SetFlightModeCapability =           1 << 0, ///< FirmwarePlugin::setFlightMode method is supported
         MavCmdPreflightStorageCapability =  1 << 1, ///< MAV_CMD_PREFLIGHT_STORAGE is supported
         PauseVehicleCapability =            1 << 2, ///< Vehicle supports pausing at current location
-        GuidedModeCapability =              1 << 3, ///< Vehicle Support guided mode commands
+        GuidedModeCapability =              1 << 3, ///< Vehicle Supports guided mode commands
+        OrbitModeCapability =               1 << 4, ///< Vehicle Supports orbit mode
     } FirmwareCapabilities;
 
     /// Maps from on parameter name to another
@@ -62,7 +63,7 @@ public:
     virtual void initializeVehicle(Vehicle* vehicle);
 
     /// @return true: Firmware supports all specified capabilites
-    virtual bool isCapable(FirmwareCapabilities capabilities);
+    virtual bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities);
 
     /// Returns VehicleComponents for specified Vehicle
     ///     @param vehicle Vehicle  to associate with components
@@ -109,6 +110,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

@@ -170,8 +170,11 @@ int PX4FirmwarePlugin::manualControlReservedButtonCount(void)
     return 0;   // 0 buttons reserved for rc switch simulation
 }
 
-bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
+bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities)
 {
+    if(vehicle->multiRotor()) {
+        return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability | OrbitModeCapability)) == capabilities;
+    }
     return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
 }
 
@@ -269,6 +272,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

@@ -30,18 +30,19 @@ public:
     QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle) final;
     QList<MAV_CMD> supportedMissionCommands(void) final;
 
-    bool        isCapable                       (FirmwareCapabilities capabilities) final;
+    bool        isCapable                       (const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
     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        pauseVehicle(Vehicle* vehicle) final;
-    void        guidedModeRTL(Vehicle* vehicle) final;
-    void        guidedModeLand(Vehicle* vehicle) final;
-    void        guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) final;
-    void        guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
-    void        guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final;
-    bool        isGuidedMode(const Vehicle* vehicle) const;
+    void        setGuidedMode                   (Vehicle* vehicle, bool guidedMode) final;
+    void        pauseVehicle                    (Vehicle* vehicle) final;
+    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;
     int         manualControlReservedButtonCount(void) final;
     void        initializeVehicle               (Vehicle* vehicle) final;
     bool        sendHomePositionToVehicle       (void) final;
@@ -52,7 +53,7 @@ public:
     QString     internalParameterMetaDataFile   (void) final { return QString(":/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml"); }
     void        getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
     QObject*    loadParameterMetaData           (const QString& metaDataFile);
-    bool        adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
+    bool        adjustIncomingMavlinkMessage    (Vehicle* vehicle, mavlink_message_t* message);
 
     // Use these constants to set flight modes using setFlightMode method. Don't use hardcoded string names since the
     // names may change.

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.orbitModeSupported && _activeVehicle.armed
+                    onClicked:  _guidedModeBar.confirmAction(_guidedModeBar.confirmOrbit)
+                }
+
             } // Row
         } // Column
     } // Rectangle - Guided mode buttons

src/Vehicle/Vehicle.cc

@@ -1223,7 +1223,7 @@ void Vehicle::setArmed(bool armed)
 
 bool Vehicle::flightModeSetAvailable(void)
 {
-    return _firmwarePlugin->isCapable(FirmwarePlugin::SetFlightModeCapability);
+    return _firmwarePlugin->isCapable(this, FirmwarePlugin::SetFlightModeCapability);
 }
 
 QStringList Vehicle::flightModes(void)
@@ -1583,12 +1583,17 @@ void Vehicle::setFlying(bool flying)
 
 bool Vehicle::guidedModeSupported(void) const
 {
-    return _firmwarePlugin->isCapable(FirmwarePlugin::GuidedModeCapability);
+    return _firmwarePlugin->isCapable(this, FirmwarePlugin::GuidedModeCapability);
 }
 
 bool Vehicle::pauseVehicleSupported(void) const
 {
-    return _firmwarePlugin->isCapable(FirmwarePlugin::PauseVehicleCapability);
+    return _firmwarePlugin->isCapable(this, FirmwarePlugin::PauseVehicleCapability);
+}
+
+bool Vehicle::orbitModeSupported() const
+{
+    return _firmwarePlugin->isCapable(this, FirmwarePlugin::OrbitModeCapability);
 }
 
 void Vehicle::guidedModeRTL(void)
@@ -1637,6 +1642,15 @@ void Vehicle::guidedModeChangeAltitude(double altitudeRel)
     _firmwarePlugin->guidedModeChangeAltitude(this, altitudeRel);
 }
 
+void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
+{
+    if (!orbitModeSupported()) {
+        qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle."));
+        return;
+    }
+    _firmwarePlugin->guidedModeOrbit(this, centerCoord, radius, velocity, altitude);
+}
+
 void Vehicle::pauseVehicle(void)
 {
     if (!pauseVehicleSupported()) {

src/Vehicle/Vehicle.h

@@ -293,6 +293,9 @@ public:
     /// true: pauseVehicle command is supported
     Q_PROPERTY(bool pauseVehicleSupported READ pauseVehicleSupported CONSTANT)
 
+    /// true: Orbit mode is supported by this vehicle
+    Q_PROPERTY(bool orbitModeSupported READ orbitModeSupported CONSTANT)
+
     // FactGroup object model properties
 
     Q_PROPERTY(Fact* roll               READ roll               CONSTANT)
@@ -349,6 +352,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);
@@ -373,6 +383,7 @@ public:
 
     bool guidedModeSupported(void) const;
     bool pauseVehicleSupported(void) const;
+    bool orbitModeSupported(void) const;
 
     // Property accessors