From d420961f49206d15b22979b5cdbdc02f0ebfbb00 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Sun, 28 May 2023 08:31:17 +0200 Subject: [PATCH] DO_SET_HOME command support: This commit adds support for setting home clicking on map. It is shown as another action in the submenu when we click over the map with an active vehicle online. As per mavlink specs, this command must use AMSL altitude, so we need to first query terrain altitude, and when received send the command. Several checks have been implemented, and in case terrain altitude is not shown at the selected location a popup will appear indicating there are no terrain data for the specified location --- src/FlightDisplay/FlyViewMap.qml | 14 ++++++- src/FlightDisplay/GuidedActionsController.qml | 12 ++++++ src/Vehicle/Vehicle.cc | 57 +++++++++++++++++++++++++++ src/Vehicle/Vehicle.h | 11 ++++++ 4 files changed, 93 insertions(+), 1 deletion(-) diff --git a/src/FlightDisplay/FlyViewMap.qml b/src/FlightDisplay/FlyViewMap.qml index 34acd23..2861e02 100644 --- a/src/FlightDisplay/FlyViewMap.qml +++ b/src/FlightDisplay/FlyViewMap.qml @@ -570,11 +570,23 @@ FlightMap { globals.guidedControllerFlyView.confirmAction(globals.guidedControllerFlyView.actionROI, clickMenu.coord, roiLocationItem) } } + + QGCButton { + Layout.fillWidth: true + text: "Set home here" + visible: globals.guidedControllerFlyView.showSetHome + onClicked: { + if (clickMenu.opened) { + clickMenu.close() + } + globals.guidedControllerFlyView.confirmAction(globals.guidedControllerFlyView.actionSetHome, clickMenu.coord) + } + } } } onClicked: { - if (!globals.guidedControllerFlyView.guidedUIVisible && (globals.guidedControllerFlyView.showGotoLocation || globals.guidedControllerFlyView.showOrbit || globals.guidedControllerFlyView.showROI)) { + if (!globals.guidedControllerFlyView.guidedUIVisible && (globals.guidedControllerFlyView.showGotoLocation || globals.guidedControllerFlyView.showOrbit || globals.guidedControllerFlyView.showROI || globals.guidedControllerFlyView.showSetHome)) { orbitMapCircle.hide() gotoLocationItem.hide() var clickCoord = _root.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */) diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 6f65023..f6ce0e2 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -56,6 +56,7 @@ Item { readonly property string gotoTitle: qsTr("Go To Location") readonly property string vtolTransitionTitle: qsTr("VTOL Transition") readonly property string roiTitle: qsTr("ROI") + readonly property string setHomeTitle: qsTr("Set Home") readonly property string actionListTitle: qsTr("Action") readonly property string armMessage: qsTr("Arm the vehicle.") @@ -81,6 +82,7 @@ Item { readonly property string vtolTransitionFwdMessage: qsTr("Transition VTOL to fixed wing flight.") readonly property string vtolTransitionMRMessage: qsTr("Transition VTOL to multi-rotor flight.") readonly property string roiMessage: qsTr("Make the specified location a Region Of Interest.") + readonly property string setHomeMessage: qsTr("Set vehicle home as the specified location. This will affect Return to Home position") readonly property int actionRTL: 1 readonly property int actionLand: 2 @@ -108,6 +110,7 @@ Item { readonly property int actionForceArm: 24 readonly property int actionChangeSpeed: 25 readonly property int actionGripper: 26 + readonly property int actionSetHome: 27 property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property bool _useChecklist: QGroundControl.settingsManager.appSettings.useChecklist.rawValue && QGroundControl.corePlugin.options.preFlightChecklistUrl.toString().length @@ -134,6 +137,7 @@ Item { property bool showROI: _guidedActionsEnabled && _vehicleFlying && __roiSupported && !_missionActive property bool showLandAbort: _guidedActionsEnabled && _vehicleFlying && _fixedWingOnApproach property bool showGotoLocation: _guidedActionsEnabled && _vehicleFlying + property bool showSetHome: _guidedActionsEnabled property bool showActionList: _guidedActionsEnabled && (showStartMission || showResumeMission || showChangeAlt || showLandAbort || actionList.hasCustomActions) property bool showGripper: _initialConnectComplete ? _activeVehicle.hasGripper : false property string changeSpeedTitle: _fixedWing ? changeAirspeedTitle : changeCruiseSpeedTitle @@ -508,6 +512,11 @@ Item { confirmDialog.message = gripperMessage _widgetLayer._gripperMenu.createObject(mainWindow).open() break + case actionSetHome: + confirmDialog.title = setHomeTitle + confirmDialog.message = setHomeMessage + confirmDialog.hideTrigger = Qt.binding(function() { return !showSetHome }) + break default: console.warn("Unknown actionCode", actionCode) return @@ -600,6 +609,9 @@ Item { case actionGripper: _gripperFunction === undefined ? _activeVehicle.sendGripperAction(Vehicle.Invalid_option) : _activeVehicle.sendGripperAction(_gripperFunction) break + case actionSetHome: + _activeVehicle.doSetHome(actionData) + break default: console.warn(qsTr("Internal error: unknown actionCode"), actionCode) break diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index a287e3b..8ae6b6b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -62,6 +62,8 @@ QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") #define UPDATE_TIMER 50 #define DEFAULT_LAT 38.965767f #define DEFAULT_LON -120.083923f +#define SET_HOME_TERRAIN_ALT_MAX 10000 +#define SET_HOME_TERRAIN_ALT_MIN -500 const QString guided_mode_not_supported_by_vehicle = QObject::tr("Guided mode not supported by Vehicle."); @@ -1482,6 +1484,7 @@ void Vehicle::_setHomePosition(QGeoCoordinate& homeCoord) { if (homeCoord != _homePosition) { _homePosition = homeCoord; + qCDebug(VehicleLog) << "new home location set at coordinate: " << homeCoord; emit homePositionChanged(_homePosition); } } @@ -4040,6 +4043,60 @@ void Vehicle::flashBootloader() } #endif +void Vehicle::doSetHome(const QGeoCoordinate& coord) +{ + if (coord.isValid()) { + // If for some reason we already did a query and it hasn't arrived yet, disconnect signals and unset current query. TerrainQuery system will + // automatically delete that forgotten query. This could happen if we send 2 do_set_home commands one after another, so usually the latest one + // is the one we would want to arrive to the vehicle, so it is fine to forget about the previous ones in case it could happen. + if (_currentDoSetHomeTerrainAtCoordinateQuery) { + disconnect(_currentDoSetHomeTerrainAtCoordinateQuery, &TerrainAtCoordinateQuery::terrainDataReceived, this, &Vehicle::_doSetHomeTerrainReceived); + _currentDoSetHomeTerrainAtCoordinateQuery = nullptr; + } + // Save the coord for using when our terrain data arrives. If there was a pending terrain query paired with an older coordinate it is safe to + // Override now, as we just disconnected the signal that would trigger the command sending + _doSetHomeCoordinate = coord; + // Now setup and trigger the new terrain query + _currentDoSetHomeTerrainAtCoordinateQuery = new TerrainAtCoordinateQuery(true /* autoDelet */); + connect(_currentDoSetHomeTerrainAtCoordinateQuery, &TerrainAtCoordinateQuery::terrainDataReceived, this, &Vehicle::_doSetHomeTerrainReceived); + QList rgCoord; + rgCoord.append(coord); + _currentDoSetHomeTerrainAtCoordinateQuery->requestData(rgCoord); + } +} + +// This will be called after our query started in doSetHome arrives +void Vehicle::_doSetHomeTerrainReceived(bool success, QList heights) +{ + if (success) { + double terrainAltitude = heights[0]; + // Coord and terrain alt sanity check + if (_doSetHomeCoordinate.isValid() && terrainAltitude <= SET_HOME_TERRAIN_ALT_MAX && terrainAltitude >= SET_HOME_TERRAIN_ALT_MIN) { + sendMavCommand( + defaultComponentId(), + MAV_CMD_DO_SET_HOME, + true, // show error if fails + 0, + 0, + 0, + static_cast(qQNaN()), + _doSetHomeCoordinate.latitude(), + _doSetHomeCoordinate.longitude(), + terrainAltitude); + + } else if (_doSetHomeCoordinate.isValid()) { + qCDebug(VehicleLog) << "_doSetHomeTerrainReceived: internal error, elevation data out of limits "; + } else { + qCDebug(VehicleLog) << "_doSetHomeTerrainReceived: internal error, cached home coordinate is not valid"; + } + } else { + qgcApp()->showAppMessage(tr("Set Home failed, terrain data not available for selected coordinate")); + } + // Clean up + _currentDoSetHomeTerrainAtCoordinateQuery = nullptr; + _doSetHomeCoordinate = QGeoCoordinate(); // So isValid() will no longer return true, for extra safety +} + void Vehicle::gimbalControlValue(double pitch, double yaw) { if (apmFirmware()) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 9a5a37d..a2ebc5a 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -47,6 +47,7 @@ #include "FTPManager.h" #include "ImageProtocolManager.h" #include "HealthAndArmingCheckReport.h" +#include "TerrainQuery.h" class Actuators; class EventHandler; @@ -459,6 +460,8 @@ public: #if !defined(NO_ARDUPILOT_DIALECT) Q_INVOKABLE void flashBootloader(); #endif + /// Set home from flight map coordinate + Q_INVOKABLE void doSetHome(const QGeoCoordinate& coord); bool isInitialConnectComplete() const; bool guidedModeSupported () const; @@ -1089,6 +1092,9 @@ private: static void _rebootCommandResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, MavCmdResultFailureCode_t failureCode); + // This is called after we get terrain data triggered from a doSetHome() + void _doSetHomeTerrainReceived (bool success, QList heights); + int _id; ///< Mavlink system id int _defaultComponentId; bool _offlineEditingVehicle = false; ///< true: This Vehicle is a "disconnected" vehicle for ui use while offline editing @@ -1448,6 +1454,11 @@ private: // Settings keys static const char* _settingsGroup; static const char* _joystickEnabledSettingsKey; + + // Terrain query members, used to get terrain altitude for doSetHome() + QTimer _updateDoSetHomeTerrainTimer; + TerrainAtCoordinateQuery* _currentDoSetHomeTerrainAtCoordinateQuery = nullptr; + QGeoCoordinate _doSetHomeCoordinate; }; Q_DECLARE_METATYPE(Vehicle::MavCmdResultFailureCode_t)