Skip to content

Commit dd48f38

Browse files
rubenp02DonLakeFlyer
rubenp02
authored and
DonLakeFlyer
committed
Support MAV_CMD_DO_REPOSITION radius parameter
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.
1 parent 1828914 commit dd48f38

13 files changed

+65
-29
lines changed

‎src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -790,7 +790,7 @@ static void _MAV_CMD_DO_REPOSITION_ResultHandler(void *resultHandlerData, int /*
790790
delete data;
791791
}
792792

793-
void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord) const
793+
void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord, double forwardFlightLoiterRadius) const
794794
{
795795
if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
796796
qgcApp()->showAppMessage(QStringLiteral("Unable to go to location, vehicle position not known."));
@@ -821,7 +821,7 @@ void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoord
821821
MAV_FRAME_GLOBAL,
822822
-1.0f,
823823
MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
824-
0.0f,
824+
static_cast<float>(forwardFlightLoiterRadius),
825825
NAN,
826826
gotoCoord.latitude(),
827827
gotoCoord.longitude(),

‎src/FirmwarePlugin/APM/APMFirmwarePlugin.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ class APMFirmwarePlugin : public FirmwarePlugin
4040
bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities) const override;
4141
void setGuidedMode(Vehicle *vehicle, bool guidedMode) const override;
4242
void guidedModeTakeoff(Vehicle *vehicle, double altitudeRel) const override;
43-
void guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate& gotoCoord) const override;
43+
void guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius) const override;
4444
double minimumTakeoffAltitudeMeters(Vehicle *vehicle) const override;
4545
void startTakeoff(Vehicle *vehicle) const override;
4646
void startMission(Vehicle *vehicle) const override;

‎src/FirmwarePlugin/FirmwarePlugin.cc

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -153,10 +153,11 @@ void FirmwarePlugin::guidedModeTakeoff(Vehicle *vehicle, double takeoffAltRel) c
153153
qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
154154
}
155155

156-
void FirmwarePlugin::guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord) const
156+
void FirmwarePlugin::guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord, double forwardFlightLoiterRadius) const
157157
{
158158
Q_UNUSED(vehicle);
159159
Q_UNUSED(gotoCoord);
160+
Q_UNUSED(forwardFlightLoiterRadius);
160161
qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
161162
}
162163

‎src/FirmwarePlugin/FirmwarePlugin.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -217,7 +217,7 @@ class FirmwarePlugin : public QObject
217217
virtual void startMission(Vehicle *vehicle) const;
218218

219219
/// Command vehicle to move to specified location (altitude is included and relative)
220-
virtual void guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord) const;
220+
virtual void guidedModeGotoLocation(Vehicle *vehicle, const QGeoCoordinate &gotoCoord, double forwardFlightLoiterRadius = 0.0) const;
221221

222222
/// Command vehicle to change altitude
223223
/// @param altitudeChange If > 0, go up by amount specified, if < 0, go down by amount specified

‎src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -400,8 +400,12 @@ bool PX4FirmwarePlugin::fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle) const
400400
vehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, "FW_AIRSPD_MAX");
401401
}
402402

403-
void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) const
403+
void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius) const
404404
{
405+
// PX4 doesn't support setting the forward flight loiter radius of
406+
// MAV_CMD_DO_REPOSITION
407+
Q_UNUSED(forwardFlightLoiterRadius)
408+
405409
if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
406410
qgcApp()->showAppMessage(tr("Unable to go to location, vehicle position not known."));
407411
return;

‎src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,7 @@ class PX4FirmwarePlugin : public FirmwarePlugin
5151
double minimumEquivalentAirspeed(Vehicle* vehicle) const override;
5252
bool mulirotorSpeedLimitsAvailable(Vehicle* vehicle) const override;
5353
bool fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle) const override;
54-
void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) const override;
54+
void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius) const override;
5555
void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeRel, bool pauseVehicle) override;
5656
void guidedModeChangeGroundSpeedMetersSecond(Vehicle* vehicle, double groundspeed) const override;
5757
void guidedModeChangeEquivalentAirspeedMetersSecond(Vehicle* vehicle, double airspeed_equiv) const override;

‎src/FlightDisplay/GuidedActionsController.qml

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -650,7 +650,12 @@ Item {
650650
_activeVehicle.guidedModeChangeAltitude(altitudeChangeInMeters, false /* pauseVehicle */)
651651
break
652652
case actionGoto:
653-
_activeVehicle.guidedModeGotoLocation(actionData)
653+
_activeVehicle.guidedModeGotoLocation(
654+
actionData,
655+
_vehicleInFwdFlight /* forwardFlightLoiterRadius */
656+
? _flyViewSettings.forwardFlightGoToLocationLoiterRad.value
657+
: 0
658+
)
654659
break
655660
case actionSetWaypoint:
656661
_activeVehicle.setCurrentMissionSequence(actionData)

‎src/Settings/FlyView.SettingsGroup.json

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,14 @@
6161
"default": 1000,
6262
"min": 1
6363
},
64+
{
65+
"name": "forwardFlightGoToLocationLoiterRad",
66+
"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.",
67+
"type": "double",
68+
"units": "m",
69+
"default": 80,
70+
"min": 25
71+
},
6472
{
6573
"name": "updateHomePosition",
6674
"shortDesc": "Send updated GCS' home position to autopilot in case of change of the home position",

‎src/Settings/FlyViewSettings.cc

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@ DECLARE_SETTINGSFACT(FlyViewSettings, showLogReplayStatusBar)
2222
DECLARE_SETTINGSFACT(FlyViewSettings, showAdditionalIndicatorsCompass)
2323
DECLARE_SETTINGSFACT(FlyViewSettings, lockNoseUpCompass)
2424
DECLARE_SETTINGSFACT(FlyViewSettings, maxGoToLocationDistance)
25+
DECLARE_SETTINGSFACT(FlyViewSettings, forwardFlightGoToLocationLoiterRad)
2526
DECLARE_SETTINGSFACT(FlyViewSettings, keepMapCenteredOnVehicle)
2627
DECLARE_SETTINGSFACT(FlyViewSettings, showSimpleCameraControl)
2728
DECLARE_SETTINGSFACT(FlyViewSettings, showObstacleDistanceOverlay)

‎src/Settings/FlyViewSettings.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@ class FlyViewSettings : public SettingsGroup
2525
DEFINE_SETTINGFACT(showAdditionalIndicatorsCompass)
2626
DEFINE_SETTINGFACT(lockNoseUpCompass)
2727
DEFINE_SETTINGFACT(maxGoToLocationDistance)
28+
DEFINE_SETTINGFACT(forwardFlightGoToLocationLoiterRad)
2829
DEFINE_SETTINGFACT(keepMapCenteredOnVehicle)
2930
DEFINE_SETTINGFACT(showSimpleCameraControl)
3031
DEFINE_SETTINGFACT(showObstacleDistanceOverlay)

‎src/UI/preferences/FlyViewSettings.qml

Lines changed: 34 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -23,23 +23,24 @@ import QGroundControl.Palette
2323
import QGroundControl.Controllers
2424

2525
SettingsPage {
26-
property var _settingsManager: QGroundControl.settingsManager
27-
property var _flyViewSettings: _settingsManager.flyViewSettings
28-
property var _mavlinkActionsSettings: _settingsManager.mavlinkActionsSettings
29-
property Fact _virtualJoystick: _settingsManager.appSettings.virtualJoystick
30-
property Fact _virtualJoystickAutoCenterThrottle: _settingsManager.appSettings.virtualJoystickAutoCenterThrottle
31-
property Fact _virtualJoystickLeftHandedMode: _settingsManager.appSettings.virtualJoystickLeftHandedMode
32-
property Fact _enableMultiVehiclePanel: _settingsManager.appSettings.enableMultiVehiclePanel
33-
property Fact _showAdditionalIndicatorsCompass: _flyViewSettings.showAdditionalIndicatorsCompass
34-
property Fact _lockNoseUpCompass: _flyViewSettings.lockNoseUpCompass
35-
property Fact _guidedMinimumAltitude: _flyViewSettings.guidedMinimumAltitude
36-
property Fact _guidedMaximumAltitude: _flyViewSettings.guidedMaximumAltitude
37-
property Fact _maxGoToLocationDistance: _flyViewSettings.maxGoToLocationDistance
38-
property var _viewer3DSettings: _settingsManager.viewer3DSettings
39-
property Fact _viewer3DEnabled: _viewer3DSettings.enabled
40-
property Fact _viewer3DOsmFilePath: _viewer3DSettings.osmFilePath
41-
property Fact _viewer3DBuildingLevelHeight: _viewer3DSettings.buildingLevelHeight
42-
property Fact _viewer3DAltitudeBias: _viewer3DSettings.altitudeBias
26+
property var _settingsManager: QGroundControl.settingsManager
27+
property var _flyViewSettings: _settingsManager.flyViewSettings
28+
property var _mavlinkActionsSettings: _settingsManager.mavlinkActionsSettings
29+
property Fact _virtualJoystick: _settingsManager.appSettings.virtualJoystick
30+
property Fact _virtualJoystickAutoCenterThrottle: _settingsManager.appSettings.virtualJoystickAutoCenterThrottle
31+
property Fact _virtualJoystickLeftHandedMode: _settingsManager.appSettings.virtualJoystickLeftHandedMode
32+
property Fact _enableMultiVehiclePanel: _settingsManager.appSettings.enableMultiVehiclePanel
33+
property Fact _showAdditionalIndicatorsCompass: _flyViewSettings.showAdditionalIndicatorsCompass
34+
property Fact _lockNoseUpCompass: _flyViewSettings.lockNoseUpCompass
35+
property Fact _guidedMinimumAltitude: _flyViewSettings.guidedMinimumAltitude
36+
property Fact _guidedMaximumAltitude: _flyViewSettings.guidedMaximumAltitude
37+
property Fact _maxGoToLocationDistance: _flyViewSettings.maxGoToLocationDistance
38+
property Fact _forwardFlightGoToLocationLoiterRad: _flyViewSettings.forwardFlightGoToLocationLoiterRad
39+
property var _viewer3DSettings: _settingsManager.viewer3DSettings
40+
property Fact _viewer3DEnabled: _viewer3DSettings.enabled
41+
property Fact _viewer3DOsmFilePath: _viewer3DSettings.osmFilePath
42+
property Fact _viewer3DBuildingLevelHeight: _viewer3DSettings.buildingLevelHeight
43+
property Fact _viewer3DAltitudeBias: _viewer3DSettings.altitudeBias
4344

4445
QGCFileDialogController { id: fileController }
4546

@@ -115,7 +116,8 @@ SettingsPage {
115116
SettingsGroupLayout {
116117
Layout.fillWidth: true
117118
heading: qsTr("Guided Commands")
118-
visible: _guidedMinimumAltitude.visible || _guidedMaximumAltitude.visible || _maxGoToLocationDistance.visible
119+
visible: _guidedMinimumAltitude.visible || _guidedMaximumAltitude.visible ||
120+
_maxGoToLocationDistance.visible || _forwardFlightGoToLocationLoiterRad.visible
119121

120122
LabelledFactTextField {
121123
Layout.fillWidth: true
@@ -137,6 +139,20 @@ SettingsPage {
137139
fact: _maxGoToLocationDistance
138140
visible: fact.visible
139141
}
142+
143+
LabelledFactTextField {
144+
Layout.fillWidth: true
145+
label: qsTr("Loiter Radius in Forward Flight Guided Mode")
146+
fact: _forwardFlightGoToLocationLoiterRad
147+
visible: fact.visible
148+
}
149+
150+
FactCheckBoxSlider {
151+
Layout.fillWidth: true
152+
text: qsTr("Require Confirmation for Go To Location in Guided Mode")
153+
fact: _goToLocationRequiresConfirmInGuided
154+
visible: fact.visible
155+
}
140156
}
141157

142158
SettingsGroupLayout {

‎src/Vehicle/Vehicle.cc

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2123,7 +2123,7 @@ void Vehicle::startMission()
21232123
_firmwarePlugin->startMission(this);
21242124
}
21252125

2126-
void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
2126+
void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius)
21272127
{
21282128
if (!guidedModeSupported()) {
21292129
qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
@@ -2137,7 +2137,7 @@ void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
21372137
qgcApp()->showAppMessage(QString("New location is too far. Must be less than %1 %2.").arg(qRound(FactMetaData::metersToAppSettingsHorizontalDistanceUnits(maxDistance).toDouble())).arg(FactMetaData::appSettingsHorizontalDistanceUnitsString()));
21382138
return;
21392139
}
2140-
_firmwarePlugin->guidedModeGotoLocation(this, gotoCoord);
2140+
_firmwarePlugin->guidedModeGotoLocation(this, gotoCoord, forwardFlightLoiterRadius);
21412141
}
21422142

21432143
void Vehicle::guidedModeChangeAltitude(double altitudeChange, bool pauseVehicle)

‎src/Vehicle/Vehicle.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -314,7 +314,7 @@ class Vehicle : public VehicleFactGroup
314314
Q_INVOKABLE double minimumEquivalentAirspeed();
315315

316316
/// Command vehicle to move to specified location (altitude is ignored)
317-
Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord);
317+
Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord, double forwardFlightLoiterRadius = 0.0f);
318318

319319
/// Command vehicle to change altitude
320320
/// @param altitudeChange If > 0, go up by amount specified, if < 0, go down by amount specified

0 commit comments

Comments
 (0)