diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 5298480d04ca72fe2d588aa23faa43f678018c54..c2f353342a8d2065545683b35bf1dd235dd1d9b3 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/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; diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 491c843e1b6a42177e2f5b5060b0ccf1d351c2c6..41aeb24ef9cf8c75e9a167b4ea0ea22a1adc906d 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/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 diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index e574c4daed6fdf447982a70a3f5d44592a4d16b4..162ccc1fe36ced2fbdd482d185a8bc7e935aaadd 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/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); } diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index badddea8a176fc1304f67e35677e8e71d9af53b6..d549fdd65286d90fc9bdeeaf29e4bd70ef440e14 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/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); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index f75fb517463b58dc39806e867ad5e88a0520ca4b..6908eeca04843ea887a0eb41d689cd5c5f22a513 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/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) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 386efd56b5d020530ad29a468ebfd21f98968557..efce082b3240271cd3ab88d5cfc45d4799d99dce 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/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; diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 4909c891acc87de916bfa939974c9ad194b21e29..b704a05799ff44aa5b17a1c652f94b1c85768b79 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/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: diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 890789e9dcbf6158f988d046ba696b182a8afb11..e27a0ed7657920a055ad7aa0b75d0e99c9f0d10f 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 9fc162a5700cac193755c47d35533ad87633a830..ed55428a64a7a15d0afb0270fc7440273b16fedf 100644 --- a/src/Vehicle/Vehicle.h +++ b/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);