diff --git a/src/FlyView/FlyViewMap.qml b/src/FlyView/FlyViewMap.qml index d6eacc939377..eb7d7d50d715 100644 --- a/src/FlyView/FlyViewMap.qml +++ b/src/FlyView/FlyViewMap.qml @@ -709,7 +709,7 @@ FlightMap { visible: globals.guidedControllerFlyView.showROI onClicked: { mapClickDropPanel.close() - globals.guidedControllerFlyView.executeAction(globals.guidedControllerFlyView.actionROI, mapClickCoord, 0, false) + globals.guidedControllerFlyView.confirmAction(globals.guidedControllerFlyView.actionROI, mapClickCoord) } } diff --git a/src/FlyView/GuidedActionsController.qml b/src/FlyView/GuidedActionsController.qml index 9f29d6bd408b..8fa694467ec2 100644 --- a/src/FlyView/GuidedActionsController.qml +++ b/src/FlyView/GuidedActionsController.qml @@ -225,7 +225,7 @@ Item { } else { console.error("setupSlider called for inapproproate change speed action", _vehicleInFwdFlight, _activeVehicle.haveMRSpeedLimits) } - } else if (actionCode === actionChangeAlt || actionCode === actionOrbit || actionCode === actionGoto || actionCode === actionPause) { + } else if (actionCode === actionChangeAlt || actionCode === actionOrbit || actionCode === actionROI || actionCode === actionGoto || actionCode === actionPause) { guidedValueSlider.setupSlider( GuidedValueSlider.SliderType.Altitude, _flyViewSettings.guidedMinimumAltitude.value, @@ -656,7 +656,12 @@ Item { } break case actionROI: - _activeVehicle.guidedModeROI(actionData) + var roiCoordinate = actionData + var valueInMeters = _unitsConversion.appSettingsVerticalDistanceUnitsToMeters(sliderOutputValue) + if (!isNaN(valueInMeters)) { + roiCoordinate = QtPositioning.coordinate(actionData.latitude, actionData.longitude, valueInMeters) + } + _activeVehicle.guidedModeROI(roiCoordinate) break case actionChangeSpeed: if (_activeVehicle) { diff --git a/src/Vehicle/TerrainQueryCoordinator.cc b/src/Vehicle/TerrainQueryCoordinator.cc index b91dd3fd10dd..7076504a2c45 100644 --- a/src/Vehicle/TerrainQueryCoordinator.cc +++ b/src/Vehicle/TerrainQueryCoordinator.cc @@ -84,7 +84,7 @@ void TerrainQueryCoordinator::_doSetHomeTerrainReceived(bool success, QList he _roiQuery = nullptr; if (!_roiCoordinate.isValid()) { + _roiRelativeAltitude = qQNaN(); return; } @@ -123,13 +125,19 @@ void TerrainQueryCoordinator::_roiTerrainReceived(bool success, QList he roiAltitude = static_cast(_vehicle->homePosition().altitude()); } + if (!qIsNaN(_roiRelativeAltitude)) { + roiAltitude += static_cast(_roiRelativeAltitude); + } + qCDebug(TerrainQueryCoordinatorLog) << "roiWithTerrain: lat" << _roiCoordinate.latitude() << "lon" << _roiCoordinate.longitude() - << "terrainAltAMSL" << roiAltitude << "success" << success; + << "altAMSL" << roiAltitude << "success" << success + << "relativeAlt" << _roiRelativeAltitude; sendROICommand(_roiCoordinate, MAV_FRAME_GLOBAL, roiAltitude); _roiCoordinate = QGeoCoordinate(); + _roiRelativeAltitude = qQNaN(); } void TerrainQueryCoordinator::sendROICommand(const QGeoCoordinate& coord, MAV_FRAME frame, float altitude) diff --git a/src/Vehicle/TerrainQueryCoordinator.h b/src/Vehicle/TerrainQueryCoordinator.h index 495afbe35e01..909362cc19cd 100644 --- a/src/Vehicle/TerrainQueryCoordinator.h +++ b/src/Vehicle/TerrainQueryCoordinator.h @@ -37,8 +37,8 @@ class TerrainQueryCoordinator : public QObject /// PX4 ROI flow. Starts a terrain query; when it resolves, sends /// MAV_CMD_DO_SET_ROI_LOCATION in MAV_FRAME_GLOBAL at the resolved altitude - /// (falling back to home altitude on query failure). - void roiWithTerrain(const QGeoCoordinate& coord); + /// or at resolved altitude plus requested relative altitude. + void roiWithTerrain(const QGeoCoordinate& coord, double relativeAltitude = qQNaN()); /// Build + send MAV_CMD_DO_SET_ROI_LOCATION directly (no terrain query). /// Used by the ArduPilot MAV_FRAME_GLOBAL_RELATIVE_ALT direct path. @@ -62,6 +62,7 @@ private slots: QPointer _roiQuery; QGeoCoordinate _roiCoordinate; + double _roiRelativeAltitude = qQNaN(); QElapsedTimer _altQueryTimer; QPointer _altQuery; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 3e0cf405f260..0c9429507946 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1985,7 +1985,7 @@ void Vehicle::guidedModeROI(const QGeoCoordinate& centerCoord) if (px4Firmware()) { // PX4 ignores the coordinate frame in COMMAND_INT and treats the altitude as AMSL, // so a terrain query is required before we can send the ROI command. - _terrainQueryCoordinator->roiWithTerrain(centerCoord); + _terrainQueryCoordinator->roiWithTerrain(centerCoord, centerCoord.altitude()); } else { // ArduPilot handles MAV_FRAME_GLOBAL_RELATIVE_ALT correctly, so altitude 0 relative to // home is a reasonable default for a map click with no altitude info.