From 16cd2a525e47290a7e37974a112caa133b87dc0e Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Tue, 31 Oct 2017 15:59:22 -0700 Subject: [PATCH] Specify altitude for guided takeoff --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 30 ++++++++++++------- src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 4 +-- src/FirmwarePlugin/FirmwarePlugin.cc | 3 +- src/FirmwarePlugin/FirmwarePlugin.h | 2 +- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 11 ++++--- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 2 +- src/FlightDisplay/GuidedActionsController.qml | 4 ++- src/Vehicle/Vehicle.cc | 4 +-- src/Vehicle/Vehicle.h | 2 +- 9 files changed, 39 insertions(+), 23 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 5298480d0..c2f353342 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 491c843e1..41aeb24ef 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 e574c4dae..162ccc1fe 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 badddea8a..d549fdd65 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 f75fb5174..6908eeca0 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 386efd56b..efce082b3 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 4909c891a..b704a0579 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 890789e9d..e27a0ed76 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 9fc162a57..ed55428a6 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); -- 2.22.0