Commit 3c252685 authored by DonLakeFlyer's avatar DonLakeFlyer

Use default minimum takeoff alt for guided takeoff

parent 890b0a5e
......@@ -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;
}
......
......@@ -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;
......
......@@ -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);
......
......@@ -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,
......
......@@ -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;
......
......@@ -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:
......
......@@ -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)
}
}
}
......
......@@ -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);
......
......@@ -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);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment