From 3c252685576fbc105feb26015f3707e2c1ff272d Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Fri, 5 Jan 2018 12:30:15 -0800 Subject: [PATCH] Use default minimum takeoff alt for guided takeoff --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 27 +++++++++---------- src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 1 + src/FirmwarePlugin/FirmwarePlugin.h | 3 +++ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 20 +++++++++----- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 1 + src/FlightDisplay/GuidedActionsController.qml | 2 +- src/FlightDisplay/GuidedAltitudeSlider.qml | 8 ++++++ src/Vehicle/Vehicle.cc | 5 ++++ src/Vehicle/Vehicle.h | 3 +++ 9 files changed, 48 insertions(+), 22 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index c2f353342..b2e399a07 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 41aeb24ef..1353e0ca0 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 353b4c79b..5a8e9b5d1 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 135969f9b..9932a1638 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 efce082b3..c0fdb450c 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 65a8ad8d5..e1a187189 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 c38c838f7..2144711d9 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 41c925eff..012e68c75 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 61cb4c69b..16be89f0f 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); -- 2.22.0