From 107aa3720145789efba890a0cf10a1e07a1e5725 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Jun 2021 15:44:07 +0200 Subject: [PATCH] PX4FirmwarePlugin: ignore minimum takeoff altitude The old behaviour adjusts the takeoff altitude set by the user with a slider to be higher than the minimum takeoff altitude as definied by the param MIS_TAKEOFF_ALT. This can be very confusing (and dangerous) as the slider to enter the altitude does not reflect that. It always goes from 0m to 100m. For example: the param is set to 10m. Now the user can command a takeoff and set the slider to 1.5m. The drone will then takeoff and climb to 10m completely without warning I don't think it makes much sense to magically change the user input without feedback. Therefore, I removed the adjustment. With this change the drone will just go to the altitude commanded by the slider. --- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 16 +--------------- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 1 - 2 files changed, 1 insertion(+), 16 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index bf49e18..165f1ae 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -389,17 +389,6 @@ void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int comm } } -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(); @@ -408,10 +397,7 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel return; } - double takeoffAltRelFromVehicle = minimumTakeoffAltitude(vehicle); - double takeoffAltAMSL = qMax(takeoffAltRel, takeoffAltRelFromVehicle) + vehicleAltitudeAMSL; - - qDebug() << takeoffAltRel << takeoffAltRelFromVehicle << takeoffAltAMSL << vehicleAltitudeAMSL; + double takeoffAltAMSL = takeoffAltRel + vehicleAltitudeAMSL; connect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult); vehicle->sendMavCommand( diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 1fa2173..5b11ea2 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -48,7 +48,6 @@ public: void guidedModeTakeoff (Vehicle* vehicle, double takeoffAltRel) override; void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override; void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeRel, bool pauseVehicle) override; - double minimumTakeoffAltitude (Vehicle* vehicle) override; void startMission (Vehicle* vehicle) override; bool isGuidedMode (const Vehicle* vehicle) const override; void initializeVehicle (Vehicle* vehicle) override;