Support MAV_CMD_DO_REPOSITION radius parameter

rubenp02 · DonLakeFlyer · commit dd48f3850c47 · 2025-05-13T12:07:04.000-07: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
@@ -790,7 +790,7 @@ static void _MAV_CMD_DO_REPOSITION_ResultHandler(void *resultHandlerData, int /*
     delete data;
 }
 
-void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord) const
+void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord, double forwardFlightLoiterRadius) const
 {
     if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
         qgcApp()->showAppMessage(QStringLiteral("Unable to go to location, vehicle position not known."));
@@ -821,7 +821,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
@@ -40,7 +40,7 @@ class APMFirmwarePlugin : public FirmwarePlugin
     bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities) const override;
     void setGuidedMode(Vehicle *vehicle, bool guidedMode) const override;
     void guidedModeTakeoff(Vehicle *vehicle, double altitudeRel) const override;
-    void guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate& gotoCoord) const override;
+    void guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius) const override;
     double minimumTakeoffAltitudeMeters(Vehicle *vehicle) const override;
     void startTakeoff(Vehicle *vehicle) const override;
     void startMission(Vehicle *vehicle) const override;
diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc
@@ -153,10 +153,11 @@ void FirmwarePlugin::guidedModeTakeoff(Vehicle *vehicle, double takeoffAltRel) c
     qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
 }
 
-void FirmwarePlugin::guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord) const
+void FirmwarePlugin::guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord, double forwardFlightLoiterRadius) const
 {
     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
@@ -217,7 +217,7 @@ class FirmwarePlugin : public QObject
     virtual void startMission(Vehicle *vehicle) const;
 
     /// Command vehicle to move to specified location (altitude is included and relative)
-    virtual void guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord) const;
+    virtual void guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord, double forwardFlightLoiterRadius = 0.0) const;
 
     /// 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
@@ -400,8 +400,12 @@ bool PX4FirmwarePlugin::fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle) const
             vehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, "FW_AIRSPD_MAX");
 }
 
-void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) const
+void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius) const
 {
+    // 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
@@ -51,7 +51,7 @@ class PX4FirmwarePlugin : public FirmwarePlugin
     double              minimumEquivalentAirspeed(Vehicle* vehicle) const override;
     bool                mulirotorSpeedLimitsAvailable(Vehicle* vehicle) const override;
     bool                fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle) const override;
-    void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) const override;
+    void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius) const override;
     void                guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeRel, bool pauseVehicle) override;
     void                guidedModeChangeGroundSpeedMetersSecond(Vehicle* vehicle, double groundspeed) const override;
     void                guidedModeChangeEquivalentAirspeedMetersSecond(Vehicle* vehicle, double airspeed_equiv) const override;
diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml
@@ -650,7 +650,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,23 +23,24 @@ 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   _virtualJoystickLeftHandedMode:     _settingsManager.appSettings.virtualJoystickLeftHandedMode
-    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   _virtualJoystickLeftHandedMode:         _settingsManager.appSettings.virtualJoystickLeftHandedMode
+    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 }
 
@@ -115,7 +116,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
@@ -137,6 +139,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
@@ -2123,7 +2123,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);
@@ -2137,7 +2137,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
@@ -314,7 +314,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
`@@ -153,10 +153,11 @@ void FirmwarePlugin::guidedModeTakeoff(Vehicle *vehicle, double takeoffAltRel) c`
`153`	`153`	`qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);`
`154`	`154`	`}`
`155`	`155`
`156`		`-void FirmwarePlugin::guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord) const`
	`156`	`+void FirmwarePlugin::guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord, double forwardFlightLoiterRadius) const`
`157`	`157`	`{`
`158`	`158`	`Q_UNUSED(vehicle);`
`159`	`159`	`Q_UNUSED(gotoCoord);`
	`160`	`+ Q_UNUSED(forwardFlightLoiterRadius);`
`160`	`161`	`qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);`
`161`	`162`	`}`
`162`	`163`
Original file line number	Diff line number	Diff line change
`@@ -400,8 +400,12 @@ bool PX4FirmwarePlugin::fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle) const`
`400`	`400`	`vehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, "FW_AIRSPD_MAX");`
`401`	`401`	`}`
`402`	`402`
`403`		`-void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) const`
	`403`	`+void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius) const`
`404`	`404`	`{`
	`405`	`+ // PX4 doesn't support setting the forward flight loiter radius of`
	`406`	`+ // MAV_CMD_DO_REPOSITION`
	`407`	`+ Q_UNUSED(forwardFlightLoiterRadius)`
	`408`	`+`
`405`	`409`	`if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {`
`406`	`410`	`qgcApp()->showAppMessage(tr("Unable to go to location, vehicle position not known."));`
`407`	`411`	`return;`