From 32dc6531b0700836714f8726c07484f4f9b65a34 Mon Sep 17 00:00:00 2001 From: Ahmad Wael Date: Thu, 2 Jul 2026 02:16:44 +0300 Subject: [PATCH] feat(waypoint radius): Add waypoint radius setting & map visual Introduce a Waypoint Radius Fact and UI/visual support. MissionSettingsItem gained a waypointRadius Fact (default 50 m) with metadata and sync logic to vehicle parameters (WP_RADIUS / WPNAV_RADIUS / NAV_ACC_RAD). MissionController now saves/loads waypointRadius in mission JSON. PlanMasterController writes the parameter to the vehicle during send. SimpleMissionItem exposes waypointRadius and triggers visual updates. QML editors (MissionDefaultsEditor, MissionSettingsEditor) and SimpleItemMapVisual add controls and an interactive map circle to view/edit radius. Unit test added to verify propagation. --- src/MissionManager/MissionController.cc | 4 + src/MissionManager/MissionSettingsItem.cc | 69 +++++++++++++++ src/MissionManager/MissionSettingsItem.h | 5 ++ src/MissionManager/PlanMasterController.cc | 21 +++++ src/MissionManager/SimpleMissionItem.cc | 31 +++++++ src/MissionManager/SimpleMissionItem.h | 7 ++ src/PlanView/MissionDefaultsEditor.qml | 7 ++ src/PlanView/MissionSettingsEditor.qml | 7 ++ src/PlanView/SimpleItemMapVisual.qml | 91 ++++++++++++++++++++ test/MissionManager/SimpleMissionItemTest.cc | 15 ++++ test/MissionManager/SimpleMissionItemTest.h | 1 + 11 files changed, 258 insertions(+) diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 66bc1a6bc824..6b0632a2586a 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -643,6 +643,9 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec } MissionSettingsItem* settingsItem = new MissionSettingsItem(_masterController, _flyView); settingsItem->setCoordinate(homeCoordinate); + if (json.contains(QStringLiteral("waypointRadius"))) { + settingsItem->waypointRadius()->setRawValue(json[QStringLiteral("waypointRadius")].toDouble()); + } visualItems->insert(0, settingsItem); qCDebug(MissionControllerLog) << "plannedHomePosition" << homeCoordinate; @@ -884,6 +887,7 @@ void MissionController::save(QJsonObject& json) json[_jsonCruiseSpeedKey] = _controllerVehicle->defaultCruiseSpeed(); json[_jsonHoverSpeedKey] = _controllerVehicle->defaultHoverSpeed(); json[_jsonGlobalPlanAltitudeModeKey] = _globalAltFrame; + json[QStringLiteral("waypointRadius")] = settingsItem->waypointRadius()->rawValue().toDouble(); // Save the visual items diff --git a/src/MissionManager/MissionSettingsItem.cc b/src/MissionManager/MissionSettingsItem.cc index 5c3dcad418e1..48e341ae5a1e 100644 --- a/src/MissionManager/MissionSettingsItem.cc +++ b/src/MissionManager/MissionSettingsItem.cc @@ -3,6 +3,7 @@ #include "MissionItem.h" #include "QGCMath.h" #include "Vehicle.h" +#include "ParameterManager.h" #include "QGCLoggingCategory.h" #include @@ -15,6 +16,7 @@ MissionSettingsItem::MissionSettingsItem(PlanMasterController* masterController, : ComplexMissionItem (masterController, flyView) , _managerVehicle (masterController->managerVehicle()) , _plannedHomePositionAltitudeFact (0, _plannedHomePositionAltitudeName, FactMetaData::valueTypeDouble) + , _waypointRadiusFact (0, "Waypoint Radius", FactMetaData::valueTypeDouble) , _cameraSection (masterController) , _speedSection (masterController) { @@ -29,6 +31,19 @@ MissionSettingsItem::MissionSettingsItem(PlanMasterController* masterController, _plannedHomePositionAltitudeFact.setRawValue (_plannedHomePositionAltitudeFact.rawDefaultValue()); setHomePositionSpecialCase(true); + FactMetaData* waypointRadiusMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this); + waypointRadiusMetaData->setRawUnits("m"); + waypointRadiusMetaData->setRawIncrement(1); + waypointRadiusMetaData->setDecimalPlaces(1); + waypointRadiusMetaData->setRawUserMin(1.0); + waypointRadiusMetaData->setRawUserMax(32767.0); + waypointRadiusMetaData->setRawDefaultValue(50.0); + _waypointRadiusFact.setMetaData(waypointRadiusMetaData); + _waypointRadiusFact.setRawValue(50.0); + + connect(masterController, &PlanMasterController::managerVehicleChanged, this, &MissionSettingsItem::_syncWaypointRadiusWithVehicle); + _syncWaypointRadiusWithVehicle(); + _cameraSection.setAvailable(true); _speedSection.setAvailable(true); @@ -243,3 +258,57 @@ void MissionSettingsItem::_updateFlyViewHomePosition(const QGeoCoordinate& homeP setCoordinate(homePosition); } } + +Fact* MissionSettingsItem::waypointRadius(void) +{ + return &_waypointRadiusFact; +} + +void MissionSettingsItem::_syncWaypointRadiusWithVehicle(void) +{ + Vehicle* vehicle = masterController()->controllerVehicle(); + if (!vehicle || !vehicle->parameterManager()) { + return; + } + + static Vehicle* lastVehicle = nullptr; + if (lastVehicle != vehicle) { + if (lastVehicle && lastVehicle->parameterManager()) { + disconnect(lastVehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &MissionSettingsItem::_syncWaypointRadiusWithVehicle); + disconnect(lastVehicle->parameterManager(), &ParameterManager::factAdded, this, &MissionSettingsItem::_syncWaypointRadiusWithVehicle); + } + lastVehicle = vehicle; + _vehicleWaypointRadiusConnected = false; + connect(vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &MissionSettingsItem::_syncWaypointRadiusWithVehicle); + connect(vehicle->parameterManager(), &ParameterManager::factAdded, this, &MissionSettingsItem::_syncWaypointRadiusWithVehicle); + } + + Fact* paramFact = nullptr; + if (vehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, QStringLiteral("WP_RADIUS"))) { + paramFact = vehicle->parameterManager()->getParameter(ParameterManager::defaultComponentId, QStringLiteral("WP_RADIUS")); + } else if (vehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, QStringLiteral("WPNAV_RADIUS"))) { + paramFact = vehicle->parameterManager()->getParameter(ParameterManager::defaultComponentId, QStringLiteral("WPNAV_RADIUS")); + } else if (vehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, QStringLiteral("NAV_ACC_RAD"))) { + paramFact = vehicle->parameterManager()->getParameter(ParameterManager::defaultComponentId, QStringLiteral("NAV_ACC_RAD")); + } + + if (paramFact && !_vehicleWaypointRadiusConnected) { + _vehicleWaypointRadiusConnected = true; + if (_waypointRadiusFact.rawValue().toDouble() != 50.0) { + paramFact->setRawValue(_waypointRadiusFact.rawValue()); + } else if (paramFact->rawValue().toDouble() > 0.0) { + _waypointRadiusFact.setRawValue(paramFact->rawValue()); + } + + connect(&_waypointRadiusFact, &Fact::valueChanged, this, [paramFact](QVariant value){ + if (paramFact->rawValue() != value) { + paramFact->setRawValue(value); + } + }); + connect(paramFact, &Fact::valueChanged, this, [this](QVariant value){ + if (_waypointRadiusFact.rawValue() != value) { + _waypointRadiusFact.setRawValue(value); + } + }); + } +} diff --git a/src/MissionManager/MissionSettingsItem.h b/src/MissionManager/MissionSettingsItem.h index c0973254528d..1b444b473641 100644 --- a/src/MissionManager/MissionSettingsItem.h +++ b/src/MissionManager/MissionSettingsItem.h @@ -19,10 +19,12 @@ class MissionSettingsItem : public ComplexMissionItem Q_PROPERTY(Fact* plannedHomePositionAltitude READ plannedHomePositionAltitude CONSTANT) Q_PROPERTY(QObject* cameraSection READ cameraSection CONSTANT) Q_PROPERTY(QObject* speedSection READ speedSection CONSTANT) + Q_PROPERTY(Fact* waypointRadius READ waypointRadius CONSTANT) Fact* plannedHomePositionAltitude (void) { return &_plannedHomePositionAltitudeFact; } CameraSection* cameraSection (void) { return &_cameraSection; } SpeedSection* speedSection (void) { return &_speedSection; } + Fact* waypointRadius (void); /// Scans the loaded items for settings items bool scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex); @@ -84,11 +86,14 @@ private slots: void _updateAltitudeInCoordinate (QVariant value); void _setHomeAltFromTerrain (double terrainAltitude); void _updateFlyViewHomePosition (const QGeoCoordinate& homePosition); + void _syncWaypointRadiusWithVehicle (void); private: Vehicle* _managerVehicle = nullptr; QGeoCoordinate _plannedHomePositionCoordinate; // Does not include altitude Fact _plannedHomePositionAltitudeFact; + Fact _waypointRadiusFact; + bool _vehicleWaypointRadiusConnected = false; int _sequenceNumber = 0; CameraSection _cameraSection; SpeedSection _speedSection; diff --git a/src/MissionManager/PlanMasterController.cc b/src/MissionManager/PlanMasterController.cc index b5c71754d53e..99c81c47851c 100644 --- a/src/MissionManager/PlanMasterController.cc +++ b/src/MissionManager/PlanMasterController.cc @@ -1,4 +1,6 @@ #include "PlanMasterController.h" +#include "MissionSettingsItem.h" +#include "ParameterManager.h" #include "AppMessages.h" #include "QGCCorePlugin.h" #include "MultiVehicleManager.h" @@ -322,6 +324,25 @@ void PlanMasterController::sendToVehicle(void) qCCritical(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called while syncInProgress"; } else { qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start mission sendToVehicle"; + + if (_missionController.visualItems() && _missionController.visualItems()->count() > 0) { + MissionSettingsItem* settingsItem = qobject_cast(_missionController.visualItems()->get(0)); + if (settingsItem && settingsItem->waypointRadius()) { + Fact* paramFact = nullptr; + if (_managerVehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, QStringLiteral("WP_RADIUS"))) { + paramFact = _managerVehicle->parameterManager()->getParameter(ParameterManager::defaultComponentId, QStringLiteral("WP_RADIUS")); + } else if (_managerVehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, QStringLiteral("WPNAV_RADIUS"))) { + paramFact = _managerVehicle->parameterManager()->getParameter(ParameterManager::defaultComponentId, QStringLiteral("WPNAV_RADIUS")); + } else if (_managerVehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, QStringLiteral("NAV_ACC_RAD"))) { + paramFact = _managerVehicle->parameterManager()->getParameter(ParameterManager::defaultComponentId, QStringLiteral("NAV_ACC_RAD")); + } + + if (paramFact) { + paramFact->setRawValue(settingsItem->waypointRadius()->rawValue()); + } + } + } + _sendGeoFence = true; _missionController.sendToVehicle(); } diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index 296d45e89c86..d0b1adf395f7 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -1,4 +1,5 @@ #include "SimpleMissionItem.h" +#include "MissionSettingsItem.h" #include "JsonParsing.h" #include "MissionCommandTree.h" #include "MissionCommandUIInfo.h" @@ -11,6 +12,7 @@ #include "CameraSection.h" #include "Vehicle.h" #include "QGCMath.h" +#include "ParameterManager.h" #include #include @@ -1156,3 +1158,32 @@ void SimpleMissionItem::_possibleRadiusChanged(void) emit loiterRadiusChanged(loiterRadius()); } } + +bool SimpleMissionItem::showWaypointRadius(void) const +{ + Fact* fact = const_cast(this)->waypointRadius(); + return command() == MAV_CMD_NAV_WAYPOINT && fact && fact->rawValue().toDouble() > 0.0; +} + +Fact* SimpleMissionItem::waypointRadius(void) +{ + Fact* fact = nullptr; + if (_masterController && _masterController->missionController() && _masterController->missionController()->visualItems()) { + QmlObjectListModel* visualItems = _masterController->missionController()->visualItems(); + if (visualItems->count() > 0) { + MissionSettingsItem* settingsItem = qobject_cast(visualItems->get(0)); + if (settingsItem) { + fact = settingsItem->waypointRadius(); + } + } + } + + if (fact && !_waypointRadiusConnected) { + _waypointRadiusConnected = true; + connect(fact, &Fact::valueChanged, this, [this](QVariant value){ + emit waypointRadiusChanged(value.toDouble()); + emit showWaypointRadiusChanged(); + }); + } + return fact; +} diff --git a/src/MissionManager/SimpleMissionItem.h b/src/MissionManager/SimpleMissionItem.h index 25fbed2abf4a..4aa4d09d4dc1 100644 --- a/src/MissionManager/SimpleMissionItem.h +++ b/src/MissionManager/SimpleMissionItem.h @@ -28,6 +28,8 @@ class SimpleMissionItem : public VisualMissionItem Q_PROPERTY(Fact* altitude READ altitude CONSTANT) ///< Altitude as specified by altitudeFrame. Not necessarily true mission item altitude Q_PROPERTY(QGroundControlQmlGlobal::AltitudeFrame altitudeFrame READ altitudeFrame WRITE setAltitudeFrame NOTIFY altitudeFrameChanged) Q_PROPERTY(Fact* amslAltAboveTerrain READ amslAltAboveTerrain CONSTANT) ///< Actual AMSL altitude for item if altitudeFrame is AltitudeFrameCalcAboveTerrain or AltitudeFrameTerrain + Q_PROPERTY(Fact* waypointRadius READ waypointRadius CONSTANT) + Q_PROPERTY(bool showWaypointRadius READ showWaypointRadius NOTIFY showWaypointRadiusChanged) Q_PROPERTY(int command READ command WRITE setCommand NOTIFY commandChanged) Q_PROPERTY(bool isLoiterItem READ isLoiterItem NOTIFY isLoiterItemChanged) Q_PROPERTY(bool showLoiterRadius READ showLoiterRadius NOTIFY showLoiterRadiusChanged) @@ -68,6 +70,8 @@ class SimpleMissionItem : public VisualMissionItem QGroundControlQmlGlobal::AltitudeFrame altitudeFrame(void) const { return _altitudeFrame; } Fact* altitude (void) { return &_altitudeFact; } Fact* amslAltAboveTerrain (void) { return &_amslAltAboveTerrainFact; } + Fact* waypointRadius (void); + bool showWaypointRadius (void) const; bool isLoiterItem (void) const; bool showLoiterRadius (void) const; double loiterRadius (void) const; @@ -147,6 +151,8 @@ class SimpleMissionItem : public VisualMissionItem void isLoiterItemChanged (void); void showLoiterRadiusChanged (void); void loiterRadiusChanged (double loiterRadius); + void waypointRadiusChanged (double waypointRadius); + void showWaypointRadiusChanged (void); private slots: void _setDirty (void); @@ -177,6 +183,7 @@ private slots: bool _rawEdit = false; bool _dirty = false; bool _ignoreDirtyChangeSignals = false; + bool _waypointRadiusConnected = false; QGeoCoordinate _mapCenterHint; SpeedSection* _speedSection = nullptr; CameraSection* _cameraSection = nullptr; diff --git a/src/PlanView/MissionDefaultsEditor.qml b/src/PlanView/MissionDefaultsEditor.qml index e5841ca47e6b..5282077e0328 100644 --- a/src/PlanView/MissionDefaultsEditor.qml +++ b/src/PlanView/MissionDefaultsEditor.qml @@ -89,6 +89,13 @@ Rectangle { fact: QGroundControl.settingsManager.appSettings.defaultMissionItemAltitude } + FactTextFieldSlider { + Layout.fillWidth: true + label: qsTr("Waypoint Radius") + fact: _root._settingsItem ? _root._settingsItem.waypointRadius : null + visible: fact !== null + } + FactTextFieldSlider { Layout.fillWidth: true label: qsTr("Flight Speed") diff --git a/src/PlanView/MissionSettingsEditor.qml b/src/PlanView/MissionSettingsEditor.qml index 9e17406a38b1..8ac26d86dc93 100644 --- a/src/PlanView/MissionSettingsEditor.qml +++ b/src/PlanView/MissionSettingsEditor.qml @@ -61,5 +61,12 @@ Rectangle { visible: _showCameraSection && cameraSection.checked } } + + FactTextFieldSlider { + Layout.fillWidth: true + label: qsTr("Waypoint Radius") + fact: root.missionItem.waypointRadius + visible: fact !== null + } } } diff --git a/src/PlanView/SimpleItemMapVisual.qml b/src/PlanView/SimpleItemMapVisual.qml index c441e75cb182..ca47361665fd 100644 --- a/src/PlanView/SimpleItemMapVisual.qml +++ b/src/PlanView/SimpleItemMapVisual.qml @@ -17,6 +17,7 @@ MissionItemMapVisualBase { if (_itemVisualShowing) { _hideItemVisuals() loiterVisualLoader.active = false + waypointRadiusVisualLoader.active = false } } @@ -24,6 +25,7 @@ MissionItemMapVisualBase { if (!_itemVisualShowing) { _showItemVisuals() loiterVisualLoader.active = true + waypointRadiusVisualLoader.active = true } } @@ -36,10 +38,25 @@ MissionItemMapVisualBase { } } + function onWaypointRadiusChanged(waypointRadius) { + if (waypointRadiusVisualLoader.item) { + waypointRadiusVisualLoader.item.handleWaypointRadiusChange() + } + } + + function onShowWaypointRadiusChanged() { + if (waypointRadiusVisualLoader.item) { + waypointRadiusVisualLoader.item.handleWaypointRadiusChange() + } + } + function onCoordinateChanged(coordinate) { if (loiterVisualLoader.item) { loiterVisualLoader.item.handleCoordinateChange() } + if (waypointRadiusVisualLoader.item) { + waypointRadiusVisualLoader.item.handleCoordinateChange() + } } } @@ -59,6 +76,22 @@ MissionItemMapVisualBase { } } + Loader { + id: waypointRadiusVisualLoader + + asynchronous: true + active: false + + sourceComponent: waypointRadiusComponent + + onLoaded: { + if (item) { + item.parent = map + map.addMapItem(item) + } + } + } + Component { id: indicatorComponent @@ -133,4 +166,62 @@ MissionItemMapVisualBase { } } } + + Component { + id: waypointRadiusComponent + + MapQuickItem { + id: waypointRadiusMapQuickItem + coordinate: _root._missionItem.coordinate + visible: _root.interactive && _missionItem.isSimpleItem && _missionItem.showWaypointRadius + + property alias blockSignals: waypointRadiusMapCircleVisuals.blockSignals + property alias radius: _mapCircle.radius + + function handleWaypointRadiusChange() { + blockSignals = true + radius.rawValue = _missionItem.waypointRadius.rawValue + blockSignals = false + } + + function handleCoordinateChange() { + coordinate = _missionItem.coordinate + } + + onCoordinateChanged: _mapCircle.center = coordinate + + sourceItem: QGCMapCircleVisuals { + id: waypointRadiusMapCircleVisuals + mapControl: _root.map + mapCircle: _mapCircle + centerDragHandleVisible: false + borderColor: _missionItem.terrainCollision ? "red" : QGroundControl.globalPalette.mapMissionTrajectory + + property bool blockSignals: false + + function updateMissionItem() { + _missionItem.waypointRadius.rawValue = _mapCircle.radius.rawValue + } + + QGCMapCircle { + id: _mapCircle + center: waypointRadiusMapQuickItem.coordinate + interactive: _root.interactive && _missionItem.isCurrentItem && map.planView + showRotation: false + } + + Connections { + target: _mapCircle.radius + function onRawValueChanged() { + if (!blockSignals) waypointRadiusMapCircleVisuals.updateMissionItem() + } + } + } + + Component.onCompleted: { + handleWaypointRadiusChange() + handleCoordinateChange() + } + } + } } diff --git a/test/MissionManager/SimpleMissionItemTest.cc b/test/MissionManager/SimpleMissionItemTest.cc index 1a734747f30f..2d0e0fc26dfe 100644 --- a/test/MissionManager/SimpleMissionItemTest.cc +++ b/test/MissionManager/SimpleMissionItemTest.cc @@ -12,6 +12,7 @@ #include "PlanMasterController.h" #include "SettingsManager.h" #include "SimpleMissionItem.h" +#include "MissionSettingsItem.h" #include "SpeedSection.h" #include "UnitTest.h" #include "Vehicle.h" @@ -25,6 +26,8 @@ void SimpleMissionItemTest::init() 20.1234567, 30.1234567, 40.1234567, 50.1234567, 60.1234567, 70.1234567, true, // autoContinue false); // isCurrentItem + MissionSettingsItem* settingsItem = new MissionSettingsItem(planController(), false /* flyView */); + planController()->missionController()->visualItems()->append(settingsItem); _simpleItem = new SimpleMissionItem(planController(), false /* flyView */, missionItem); // It's important to check that the right signals are emitted at the right time since that drives ui change. // It's also important to check that things are not being over-signalled when they should not be. @@ -328,6 +331,18 @@ void SimpleMissionItemTest::_testAltitudePropogation() QCOMPARE(_simpleItem->missionItem().frame(), MAV_FRAME_GLOBAL); } +void SimpleMissionItemTest::_testWaypointRadiusPropogation() +{ + Fact* fact = _simpleItem->waypointRadius(); + QVERIFY(fact != nullptr); + QCOMPARE(fact->name(), QStringLiteral("Waypoint Radius")); + + double originalVal = fact->rawValue().toDouble(); + fact->setRawValue(12.34); + QCOMPARE(fact->rawValue().toDouble(), 12.34); + fact->setRawValue(originalVal); +} + void SimpleMissionItemTest::_testCalcAboveTerrainSaveLoad() { // Regression test for https://github.com/mavlink/qgroundcontrol/issues/13513 diff --git a/test/MissionManager/SimpleMissionItemTest.h b/test/MissionManager/SimpleMissionItemTest.h index d7d79301c062..464cda2ce019 100644 --- a/test/MissionManager/SimpleMissionItemTest.h +++ b/test/MissionManager/SimpleMissionItemTest.h @@ -28,6 +28,7 @@ private slots: void _testCameraSection(); void _testSpeedSection(); void _testAltitudePropogation(); + void _testWaypointRadiusPropogation(); void _testCalcAboveTerrainSaveLoad(); private: