Merge pull request #5811 from DonLakeFlyer/TakeoffAlt

Specify altitude for guided takeoff

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -906,30 +906,40 @@ void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
     vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
 }
 
-void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
+void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
 {
-    _guidedModeTakeoff(vehicle);
+    _guidedModeTakeoff(vehicle, altitudeRel);
 }
 
-bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
+bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
 {
     if (!vehicle->multiRotor() && !vehicle->vtol()) {
         qgcApp()->showMessage(tr("Vehicle does not support guided takeoff"));
         return false;
     }
 
+    double vehicleAltitudeAMSL = vehicle->altitudeAMSL()->rawValue().toDouble();
+    if (qIsNaN(vehicleAltitudeAMSL)) {
+        qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known."));
+        return false;
+    }
+
     QString takeoffAltParam(vehicle->vtol() ? QStringLiteral("Q_RTL_ALT") : QStringLiteral("PILOT_TKOFF_ALT"));
     float paramDivisor = vehicle->vtol() ? 1.0 : 100.0; // PILOT_TAKEOFF_ALT is in centimeters
 
-    float takeoffAlt = 0;
+    float takeoffAltRel = 0;
     if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
         Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
-        takeoffAlt = takeoffAltFact->rawValue().toDouble();
+        takeoffAltRel = takeoffAltFact->rawValue().toDouble();
     }
-    if (takeoffAlt <= 0) {
-        takeoffAlt = 2.5;
+    if (takeoffAltRel <= 0) {
+        takeoffAltRel = 2.5;
     } else {
-        takeoffAlt /= paramDivisor;   // centimeters -> meters
+        takeoffAltRel /= paramDivisor;   // centimeters -> meters
+    }
+
+    if (!qIsNaN(altitudeRel) && altitudeRel > takeoffAltRel) {
+        takeoffAltRel = altitudeRel;
     }
 
     if (!_setFlightModeAndValidate(vehicle, "Guided")) {
@@ -947,7 +957,7 @@ bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
                             MAV_CMD_NAV_TAKEOFF,
                             true, // show error
                             0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
-                            takeoffAlt);
+                            takeoffAltRel);                     // Relative altitude
 
     return true;
 }
@@ -958,7 +968,7 @@ void APMFirmwarePlugin::startMission(Vehicle* vehicle)
     double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble();
 
     if (!vehicle->flying()) {
-        if (_guidedModeTakeoff(vehicle)) {
+        if (_guidedModeTakeoff(vehicle, qQNaN())) {
 
             // Wait for vehicle to get off ground before switching to auto (10 seconds)
             bool didTakeoff = false;

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -76,7 +76,7 @@ public:
     AutoPilotPlugin*    autopilotPlugin                 (Vehicle* vehicle) override;
     bool                isCapable                       (const Vehicle *vehicle, FirmwareCapabilities capabilities) override;
     void                setGuidedMode                   (Vehicle* vehicle, bool guidedMode) override;
-    void                guidedModeTakeoff               (Vehicle* vehicle) override;
+    void                guidedModeTakeoff               (Vehicle* vehicle, double altitudeRel) override;
     void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override;
     void                startMission                    (Vehicle* vehicle) override;
     QStringList         flightModes                     (Vehicle* vehicle) override;
@@ -123,7 +123,7 @@ private:
     void _handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message);
     void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message);
     void _soloVideoHandshake(Vehicle* vehicle);    
-    bool _guidedModeTakeoff(Vehicle* vehicle);
+    bool _guidedModeTakeoff(Vehicle* vehicle, double altitudeRel);
 
     // Any instance data here must be global to all vehicles
     // Vehicle specific data should go into APMFirmwarePluginInstanceData

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -256,10 +256,11 @@ void FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
     qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
 }
 
-void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
+void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel)
 {
     // Not supported by generic vehicle
     Q_UNUSED(vehicle);
+    Q_UNUSED(takeoffAltRel);
     qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
 }
 

src/FirmwarePlugin/FirmwarePlugin.h

@@ -128,7 +128,7 @@ public:
     virtual void guidedModeLand(Vehicle* vehicle);
 
     /// Command vehicle to takeoff from current location to a firmware specific height.
-    virtual void guidedModeTakeoff(Vehicle* vehicle);
+    virtual void guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel);
 
     /// Command the vehicle to start the mission
     virtual void startMission(Vehicle* vehicle);

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -383,11 +383,12 @@ void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int comm
     }
 }
 
-void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
+void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel)
 {
     QString takeoffAltParam("MIS_TAKEOFF_ALT");
 
-    if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
+    double vehicleAltitudeAMSL = vehicle->altitudeAMSL()->rawValue().toDouble();
+    if (qIsNaN(vehicleAltitudeAMSL)) {
         qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known."));
         return;
     }
@@ -396,7 +397,9 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
         qgcApp()->showMessage(tr("Unable to takeoff, MIS_TAKEOFF_ALT parameter missing."));
         return;
     }
-    Fact* takeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
+
+    double takeoffAltRelFromVehicle = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam)->rawValue().toDouble();
+    double takeoffAltAMSL = qMax(takeoffAltRel, takeoffAltRelFromVehicle) + vehicleAltitudeAMSL;
 
     connect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult);
     vehicle->sendMavCommand(vehicle->defaultComponentId(),
@@ -405,7 +408,7 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
                             -1,                             // No pitch requested
                             0, 0,                           // param 2-4 unused
                             NAN, NAN, NAN,                  // No yaw, lat, lon
-                            vehicle->altitudeAMSL()->rawValue().toDouble() + takeoffAlt->rawValue().toDouble());
+                            takeoffAltAMSL);                // AMSL altitude
 }
 
 void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -45,7 +45,7 @@ public:
     void                pauseVehicle                    (Vehicle* vehicle) override;
     void                guidedModeRTL                   (Vehicle* vehicle) override;
     void                guidedModeLand                  (Vehicle* vehicle) override;
-    void                guidedModeTakeoff               (Vehicle* vehicle) override;
+    void                guidedModeTakeoff               (Vehicle* vehicle, double takeoffAltRel) override;
     void                guidedModeOrbit                 (Vehicle* vehicle, const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN) override;
     void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override;
     void                guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeRel) override;

src/FlightDisplay/GuidedActionsController.qml

@@ -208,6 +208,8 @@ Item {
             confirmDialog.title = takeoffTitle
             confirmDialog.message = takeoffMessage
             confirmDialog.hideTrigger = Qt.binding(function() { return !showTakeoff })
+            altitudeSlider.reset()
+            altitudeSlider.visible = true
             break;
         case actionStartMission:
             confirmDialog.title = startMissionTitle
@@ -302,7 +304,7 @@ Item {
             _activeVehicle.guidedModeLand()
             break
         case actionTakeoff:
-            _activeVehicle.guidedModeTakeoff()
+            _activeVehicle.guidedModeTakeoff(actionData)
             break
         case actionResumeMission:
         case actionResumeMissionUploadFail:

src/Vehicle/Vehicle.cc

@@ -2222,14 +2222,14 @@ void Vehicle::guidedModeLand(void)
     _firmwarePlugin->guidedModeLand(this);
 }
 
-void Vehicle::guidedModeTakeoff(void)
+void Vehicle::guidedModeTakeoff(double altitudeRelative)
 {
     if (!guidedModeSupported()) {
         qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
         return;
     }
     setGuidedMode(true);
-    _firmwarePlugin->guidedModeTakeoff(this);
+    _firmwarePlugin->guidedModeTakeoff(this, altitudeRelative);
 }
 
 void Vehicle::startMission(void)

src/Vehicle/Vehicle.h

@@ -386,7 +386,7 @@ public:
     Q_INVOKABLE void guidedModeLand(void);
 
     /// Command vehicle to takeoff from current location
-    Q_INVOKABLE void guidedModeTakeoff(void);
+    Q_INVOKABLE void guidedModeTakeoff(double altitudeRelative);
 
     /// Command vehicle to move to specified location (altitude is included and relative)
     Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord);