diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 66bc1a6bc82..6b0632a2586 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 5c3dcad418e..48e341ae5a1 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 c0973254528..1b444b47364 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 b5c71754d53..99c81c47851 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 296d45e89c8..d0b1adf395f 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 25fbed2abf4..4aa4d09d4dc 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 e5841ca47e6..5282077e032 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 9e17406a38b..8ac26d86dc9 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 c441e75cb18..ca47361665f 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 1a734747f30..2d0e0fc26df 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 d7d79301c06..464cda2ce01 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: