diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index c2f353342a8d2065545683b35bf1dd235dd1d9b3..b2e399a078341348d23319e4125ffff1fae56d4a 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -911,6 +911,18 @@ void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) _guidedModeTakeoff(vehicle, altitudeRel); } +double APMFirmwarePlugin::minimumTakeoffAltitude(Vehicle* vehicle) +{ + 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 + + if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) { + return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam)->rawValue().toDouble() / paramDivisor; + } else { + return FirmwarePlugin::minimumTakeoffAltitude(vehicle); + } +} + bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) { if (!vehicle->multiRotor() && !vehicle->vtol()) { @@ -924,20 +936,7 @@ bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) 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 takeoffAltRel = 0; - if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) { - Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam); - takeoffAltRel = takeoffAltFact->rawValue().toDouble(); - } - if (takeoffAltRel <= 0) { - takeoffAltRel = 2.5; - } else { - takeoffAltRel /= paramDivisor; // centimeters -> meters - } - + double takeoffAltRel = minimumTakeoffAltitude(vehicle); if (!qIsNaN(altitudeRel) && altitudeRel > takeoffAltRel) { takeoffAltRel = altitudeRel; } diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 41aeb24ef9cf8c75e9a167b4ea0ea22a1adc906d..1353e0ca087947550cbf0b5a33bb0920bc312bd7 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -78,6 +78,7 @@ public: void setGuidedMode (Vehicle* vehicle, bool guidedMode) override; void guidedModeTakeoff (Vehicle* vehicle, double altitudeRel) override; void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override; + double minimumTakeoffAltitude (Vehicle* vehicle) override; void startMission (Vehicle* vehicle) override; QStringList flightModes (Vehicle* vehicle) override; QString flightMode (uint8_t base_mode, uint32_t custom_mode) const override; diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 353b4c79b271d1d33808c30ae1f57d80563aa833..5a8e9b5d1c7311586d85b4c817051402696a9012 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -130,6 +130,9 @@ public: /// Command vehicle to takeoff from current location to a firmware specific height. virtual void guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel); + /// @return The minimum takeoff altitude (relative) for guided takeoff. + virtual double minimumTakeoffAltitude(Vehicle* vehicle) { Q_UNUSED(vehicle); return 10; } + /// Command the vehicle to start the mission virtual void startMission(Vehicle* vehicle); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 135969f9bab83fc7ea15fdcfc08271b92ceec7f0..9932a1638a369d04ecf1d5dd3095b457c12a6016 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -384,24 +384,30 @@ void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int comm } } -void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel) +double PX4FirmwarePlugin::minimumTakeoffAltitude(Vehicle* vehicle) { QString takeoffAltParam("MIS_TAKEOFF_ALT"); + if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) { + return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam)->rawValue().toDouble(); + } else { + return FirmwarePlugin::minimumTakeoffAltitude(vehicle); + } +} + +void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel) +{ double vehicleAltitudeAMSL = vehicle->altitudeAMSL()->rawValue().toDouble(); if (qIsNaN(vehicleAltitudeAMSL)) { qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known.")); return; } - if (!vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) { - qgcApp()->showMessage(tr("Unable to takeoff, MIS_TAKEOFF_ALT parameter missing.")); - return; - } - - double takeoffAltRelFromVehicle = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam)->rawValue().toDouble(); + double takeoffAltRelFromVehicle = minimumTakeoffAltitude(vehicle); double takeoffAltAMSL = qMax(takeoffAltRel, takeoffAltRelFromVehicle) + vehicleAltitudeAMSL; + qDebug() << takeoffAltRel << takeoffAltRelFromVehicle << takeoffAltAMSL << vehicleAltitudeAMSL; + connect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult); vehicle->sendMavCommand(vehicle->defaultComponentId(), MAV_CMD_NAV_TAKEOFF, diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index efce082b3240271cd3ab88d5cfc45d4799d99dce..c0fdb450c4213392fb8506a77979def90a4f2128 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -49,6 +49,7 @@ public: 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; + double minimumTakeoffAltitude (Vehicle* vehicle) override; void startMission (Vehicle* vehicle) override; bool isGuidedMode (const Vehicle* vehicle) const override; int manualControlReservedButtonCount(void) override; diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 65a8ad8d518f966752587c460347d73b746c16ea..e1a18718912cdb60b329fe6cacb080d87b99431e 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -213,7 +213,7 @@ Item { confirmDialog.title = takeoffTitle confirmDialog.message = takeoffMessage confirmDialog.hideTrigger = Qt.binding(function() { return !showTakeoff }) - altitudeSlider.reset() + altitudeSlider.setToMinimumTakeoff() altitudeSlider.visible = true break; case actionStartMission: diff --git a/src/FlightDisplay/GuidedAltitudeSlider.qml b/src/FlightDisplay/GuidedAltitudeSlider.qml index c38c838f7ce2cca18ba28583f23f885395ba6878..2144711d93392d98700045ca28b2e50b3858966e 100644 --- a/src/FlightDisplay/GuidedAltitudeSlider.qml +++ b/src/FlightDisplay/GuidedAltitudeSlider.qml @@ -32,6 +32,10 @@ Rectangle { altSlider.value = 0 } + function setToMinimumTakeoff() { + altField.setToMinimumTakeoff() + } + function getValue() { return altField.newAltitudeMeters - _vehicleAltitude } @@ -70,6 +74,10 @@ Rectangle { property real altLossGain: altExp * (altSlider.value > 0 ? altGainRange : altLossRange) property real newAltitudeMeters: _vehicleAltitude + altLossGain property string newAltitudeAppUnits: QGroundControl.metersToAppSettingsDistanceUnits(newAltitudeMeters).toFixed(1) + + function setToMinimumTakeoff() { + altSlider.value = Math.pow(_activeVehicle.minimumTakeoffAltitude() / altGainRange, 1.0/3.0) + } } } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 41c925eff4fbd7993f2823318cf47b4933598c08..012e68c75f8f63efbcd4409cb880f81377cc7465 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2266,6 +2266,11 @@ void Vehicle::guidedModeTakeoff(double altitudeRelative) _firmwarePlugin->guidedModeTakeoff(this, altitudeRelative); } +double Vehicle::minimumTakeoffAltitude(void) +{ + return _firmwarePlugin->minimumTakeoffAltitude(this); +} + void Vehicle::startMission(void) { _firmwarePlugin->startMission(this); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 61cb4c69b6dc65ff7b96e63a35e766939a2f834f..16be89f0fdf8f190233e488aa6bdfd8c7082146d 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -423,6 +423,9 @@ public: /// Command vehicle to takeoff from current location Q_INVOKABLE void guidedModeTakeoff(double altitudeRelative); + /// @return The minimum takeoff altitude (relative) for guided takeoff. + Q_INVOKABLE double minimumTakeoffAltitude(void); + /// Command vehicle to move to specified location (altitude is included and relative) Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord);