Support MAV_CMD_DO_REPOSITION radius parameter

rubenp02 · rubenp02 · commit 772af12aa20a · 2025-03-04T14:06:13.000+01:00
Added support for the radius parameter of the MAV_CMD_DO_REPOSITION
command. When in forward flight, this parameter can be used to specify
the radius of the orbit around the Go here position in Guided Mode. A
Fly View setting has been added to control this radius.

Key changes:
- Modified the guidedModeGotoLocation method across all firmware plugins
  to accept an optional forwardFlightLoiterRadius parameter, which sets
  the desired loiter radius during forward flight.
- Added "Loiter Radius in Forward Flight" setting to the Guided Commands
  section of the FlyViewSettings to control the default radius.

Note: PX4 doesn't doesn't handle the radius parameter per the MAVLink
spec and therefore doesn't support this functionality.
diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
@@ -798,7 +798,7 @@ static void _MAV_CMD_DO_REPOSITION_ResultHandler(void* resultHandlerData, int /*
     delete data;
 }
 
-void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
+void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius)
 {
     if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
         qgcApp()->showAppMessage(QStringLiteral("Unable to go to location, vehicle position not known."));
@@ -830,7 +830,7 @@ void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord
                 MAV_FRAME_GLOBAL,
                 -1.0f,
                 MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
-                0.0f,
+                static_cast<float>(forwardFlightLoiterRadius),
                 NAN,
                 gotoCoord.latitude(),
                 gotoCoord.longitude(),
diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
@@ -44,7 +44,7 @@ class APMFirmwarePlugin : public FirmwarePlugin
     bool                isCapable                       (const Vehicle *vehicle, FirmwareCapabilities capabilities) override;
     void                setGuidedMode                   (Vehicle* vehicle, bool guidedMode) override;
     void                guidedModeTakeoff               (Vehicle* vehicle, double altitudeRel) override;
-    void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override;
+    void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius) override;
     double              minimumTakeoffAltitudeMeters    (Vehicle* vehicle) override;
     void                startMission                    (Vehicle* vehicle) override;
     QStringList         flightModes                     (Vehicle* vehicle) override;
diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc
@@ -236,11 +236,12 @@ void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel)
     qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
 }
 
-void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
+void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius)
 {
     // Not supported by generic vehicle
     Q_UNUSED(vehicle);
     Q_UNUSED(gotoCoord);
+    Q_UNUSED(forwardFlightLoiterRadius);
     qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
 }
 
diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h
@@ -221,7 +221,7 @@ class FirmwarePlugin : public QObject
     virtual void startMission(Vehicle* vehicle);
 
     /// Command vehicle to move to specified location (altitude is included and relative)
-    virtual void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord);
+    virtual void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius = 0.0);
 
     /// Command vehicle to change altitude
     ///     @param altitudeChange If > 0, go up by amount specified, if < 0, go down by amount specified
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
@@ -407,8 +407,12 @@ bool PX4FirmwarePlugin::fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle)
             vehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, "FW_AIRSPD_MAX");
 }
 
-void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
+void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius)
 {
+    // PX4 doesn't support setting the forward flight loiter radius of
+    // MAV_CMD_DO_REPOSITION
+    Q_UNUSED(forwardFlightLoiterRadius)
+
     if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
         qgcApp()->showAppMessage(tr("Unable to go to location, vehicle position not known."));
         return;
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
@@ -52,7 +52,7 @@ class PX4FirmwarePlugin : public FirmwarePlugin
     double              minimumEquivalentAirspeed(Vehicle* vehicle) override;
     bool                mulirotorSpeedLimitsAvailable(Vehicle* vehicle) override;
     bool                fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle) override;
-    void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override;
+    void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius) override;
     void                guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeRel, bool pauseVehicle) override;
     void                guidedModeChangeGroundSpeedMetersSecond(Vehicle* vehicle, double groundspeed) override;
     void                guidedModeChangeEquivalentAirspeedMetersSecond(Vehicle* vehicle, double airspeed_equiv) override;
diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml
@@ -646,7 +646,12 @@ Item {
             _activeVehicle.guidedModeChangeAltitude(altitudeChangeInMeters, false /* pauseVehicle */)
             break
         case actionGoto:
-            _activeVehicle.guidedModeGotoLocation(actionData)
+            _activeVehicle.guidedModeGotoLocation(
+                actionData,
+                _vehicleInFwdFlight /* forwardFlightLoiterRadius */
+                    ? _flyViewSettings.forwardFlightGoToLocationLoiterRad.value
+                    : 0
+            )
             break
         case actionSetWaypoint:
             _activeVehicle.setCurrentMissionSequence(actionData)
diff --git a/src/Settings/FlyView.SettingsGroup.json b/src/Settings/FlyView.SettingsGroup.json
@@ -61,6 +61,14 @@
     "default":     1000,
     "min":              1
 },
+{
+    "name":         "forwardFlightGoToLocationLoiterRad",
+    "shortDesc":    "Loiter radius for orbiting the Go To Location during forward flight. This only applies if the firmware supports a radius in MAV_CMD_DO_REPOSITION commands.",
+    "type":         "double",
+    "units":        "m",
+    "default":      80,
+    "min":          25
+},
 {
     "name":             "updateHomePosition",
     "shortDesc": "Send updated GCS' home position to autopilot in case of change of the home position",
diff --git a/src/Settings/FlyViewSettings.cc b/src/Settings/FlyViewSettings.cc
@@ -22,6 +22,7 @@ DECLARE_SETTINGSFACT(FlyViewSettings, showLogReplayStatusBar)
 DECLARE_SETTINGSFACT(FlyViewSettings, showAdditionalIndicatorsCompass)
 DECLARE_SETTINGSFACT(FlyViewSettings, lockNoseUpCompass)
 DECLARE_SETTINGSFACT(FlyViewSettings, maxGoToLocationDistance)
+DECLARE_SETTINGSFACT(FlyViewSettings, forwardFlightGoToLocationLoiterRad)
 DECLARE_SETTINGSFACT(FlyViewSettings, keepMapCenteredOnVehicle)
 DECLARE_SETTINGSFACT(FlyViewSettings, showSimpleCameraControl)
 DECLARE_SETTINGSFACT(FlyViewSettings, showObstacleDistanceOverlay)
diff --git a/src/Settings/FlyViewSettings.h b/src/Settings/FlyViewSettings.h
@@ -25,6 +25,7 @@ class FlyViewSettings : public SettingsGroup
     DEFINE_SETTINGFACT(showAdditionalIndicatorsCompass)
     DEFINE_SETTINGFACT(lockNoseUpCompass)
     DEFINE_SETTINGFACT(maxGoToLocationDistance)
+    DEFINE_SETTINGFACT(forwardFlightGoToLocationLoiterRad)
     DEFINE_SETTINGFACT(keepMapCenteredOnVehicle)
     DEFINE_SETTINGFACT(showSimpleCameraControl)
     DEFINE_SETTINGFACT(showObstacleDistanceOverlay)
diff --git a/src/UI/preferences/FlyViewSettings.qml b/src/UI/preferences/FlyViewSettings.qml
@@ -23,22 +23,23 @@ import QGroundControl.Palette
 import QGroundControl.Controllers
 
 SettingsPage {
-    property var    _settingsManager:                   QGroundControl.settingsManager
-    property var    _flyViewSettings:                   _settingsManager.flyViewSettings
-    property var    _mavlinkActionsSettings:            _settingsManager.mavlinkActionsSettings
-    property Fact   _virtualJoystick:                   _settingsManager.appSettings.virtualJoystick
-    property Fact   _virtualJoystickAutoCenterThrottle: _settingsManager.appSettings.virtualJoystickAutoCenterThrottle
-    property Fact   _enableMultiVehiclePanel:           _settingsManager.appSettings.enableMultiVehiclePanel
-    property Fact   _showAdditionalIndicatorsCompass:   _flyViewSettings.showAdditionalIndicatorsCompass
-    property Fact   _lockNoseUpCompass:                 _flyViewSettings.lockNoseUpCompass
-    property Fact   _guidedMinimumAltitude:             _flyViewSettings.guidedMinimumAltitude
-    property Fact   _guidedMaximumAltitude:             _flyViewSettings.guidedMaximumAltitude
-    property Fact   _maxGoToLocationDistance:           _flyViewSettings.maxGoToLocationDistance
-    property var    _viewer3DSettings:                  _settingsManager.viewer3DSettings
-    property Fact   _viewer3DEnabled:                   _viewer3DSettings.enabled
-    property Fact   _viewer3DOsmFilePath:               _viewer3DSettings.osmFilePath
-    property Fact   _viewer3DBuildingLevelHeight:       _viewer3DSettings.buildingLevelHeight
-    property Fact   _viewer3DAltitudeBias:              _viewer3DSettings.altitudeBias
+    property var    _settingsManager:                       QGroundControl.settingsManager
+    property var    _flyViewSettings:                       _settingsManager.flyViewSettings
+    property var    _mavlinkActionsSettings:                _settingsManager.mavlinkActionsSettings
+    property Fact   _virtualJoystick:                       _settingsManager.appSettings.virtualJoystick
+    property Fact   _virtualJoystickAutoCenterThrottle:     _settingsManager.appSettings.virtualJoystickAutoCenterThrottle
+    property Fact   _enableMultiVehiclePanel:               _settingsManager.appSettings.enableMultiVehiclePanel
+    property Fact   _showAdditionalIndicatorsCompass:       _flyViewSettings.showAdditionalIndicatorsCompass
+    property Fact   _lockNoseUpCompass:                     _flyViewSettings.lockNoseUpCompass
+    property Fact   _guidedMinimumAltitude:                 _flyViewSettings.guidedMinimumAltitude
+    property Fact   _guidedMaximumAltitude:                 _flyViewSettings.guidedMaximumAltitude
+    property Fact   _maxGoToLocationDistance:               _flyViewSettings.maxGoToLocationDistance
+    property Fact   _forwardFlightGoToLocationLoiterRad:    _flyViewSettings.forwardFlightGoToLocationLoiterRad
+    property var    _viewer3DSettings:                      _settingsManager.viewer3DSettings
+    property Fact   _viewer3DEnabled:                       _viewer3DSettings.enabled
+    property Fact   _viewer3DOsmFilePath:                   _viewer3DSettings.osmFilePath
+    property Fact   _viewer3DBuildingLevelHeight:           _viewer3DSettings.buildingLevelHeight
+    property Fact   _viewer3DAltitudeBias:                  _viewer3DSettings.altitudeBias
 
     QGCFileDialogController { id: fileController }
 
@@ -114,7 +115,8 @@ SettingsPage {
     SettingsGroupLayout {
         Layout.fillWidth:   true
         heading:            qsTr("Guided Commands")
-        visible:            _guidedMinimumAltitude.visible || _guidedMaximumAltitude.visible || _maxGoToLocationDistance.visible
+        visible:            _guidedMinimumAltitude.visible || _guidedMaximumAltitude.visible ||
+                            _maxGoToLocationDistance.visible || _forwardFlightGoToLocationLoiterRad.visible
 
         LabelledFactTextField {
             Layout.fillWidth:   true
@@ -136,6 +138,20 @@ SettingsPage {
             fact:               _maxGoToLocationDistance
             visible:            fact.visible
         }
+
+        LabelledFactTextField {
+            Layout.fillWidth:   true
+            label:              qsTr("Loiter Radius in Forward Flight Guided Mode")
+            fact:               _forwardFlightGoToLocationLoiterRad
+            visible:            fact.visible
+        }
+
+        FactCheckBoxSlider {
+            Layout.fillWidth:   true
+            text:               qsTr("Require Confirmation for Go To Location in Guided Mode")
+            fact:               _goToLocationRequiresConfirmInGuided
+            visible:            fact.visible
+        }
     }
 
     SettingsGroupLayout {
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
@@ -2111,7 +2111,7 @@ void Vehicle::startMission()
     _firmwarePlugin->startMission(this);
 }
 
-void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
+void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius)
 {
     if (!guidedModeSupported()) {
         qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
@@ -2125,7 +2125,7 @@ void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
         qgcApp()->showAppMessage(QString("New location is too far. Must be less than %1 %2.").arg(qRound(FactMetaData::metersToAppSettingsHorizontalDistanceUnits(maxDistance).toDouble())).arg(FactMetaData::appSettingsHorizontalDistanceUnitsString()));
         return;
     }
-    _firmwarePlugin->guidedModeGotoLocation(this, gotoCoord);
+    _firmwarePlugin->guidedModeGotoLocation(this, gotoCoord, forwardFlightLoiterRadius);
 }
 
 void Vehicle::guidedModeChangeAltitude(double altitudeChange, bool pauseVehicle)
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
@@ -313,7 +313,7 @@ class Vehicle : public VehicleFactGroup
     Q_INVOKABLE double minimumEquivalentAirspeed();
 
     /// Command vehicle to move to specified location (altitude is ignored)
-    Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord);
+    Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius = 0.0f);
 
     /// Command vehicle to change altitude
     ///     @param altitudeChange If > 0, go up by amount specified, if < 0, go down by amount specified

Original file line number	Diff line number	Diff line change
`@@ -236,11 +236,12 @@ void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel)`
`236`	`236`	`qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);`
`237`	`237`	`}`
`238`	`238`
`239`		`-void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)`
	`239`	`+void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius)`
`240`	`240`	`{`
`241`	`241`	`// Not supported by generic vehicle`
`242`	`242`	`Q_UNUSED(vehicle);`
`243`	`243`	`Q_UNUSED(gotoCoord);`
	`244`	`+ Q_UNUSED(forwardFlightLoiterRadius);`
`244`	`245`	`qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);`
`245`	`246`	`}`
`246`	`247`
Original file line number	Diff line number	Diff line change
`@@ -407,8 +407,12 @@ bool PX4FirmwarePlugin::fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle)`
`407`	`407`	`vehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, "FW_AIRSPD_MAX");`
`408`	`408`	`}`
`409`	`409`
`410`		`-void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)`
	`410`	`+void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius)`
`411`	`411`	`{`
	`412`	`+ // PX4 doesn't support setting the forward flight loiter radius of`
	`413`	`+ // MAV_CMD_DO_REPOSITION`
	`414`	`+ Q_UNUSED(forwardFlightLoiterRadius)`
	`415`	`+`
`412`	`416`	`if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {`
`413`	`417`	`qgcApp()->showAppMessage(tr("Unable to go to location, vehicle position not known."));`
`414`	`418`	`return;`