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)