Browse Source

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.
QGC4.4
Julian Oes 4 years ago committed by Don Gagne
parent
commit
107aa37201
  1. 16
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
  2. 1
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

16
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@ -389,17 +389,6 @@ void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int comm @@ -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 @@ -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(

1
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@ -48,7 +48,6 @@ public: @@ -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;

Loading…
Cancel
Save