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

@@ -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 */)

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

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<QGeoCoordinate> rgCoord;
+        rgCoord.append(coord);
+        _currentDoSetHomeTerrainAtCoordinateQuery->requestData(rgCoord);
+    }
+}
+
+// This will be called after our query started in doSetHome arrives
+void Vehicle::_doSetHomeTerrainReceived(bool success, QList<double> 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<float>(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()) {

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<double> 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)