From cba2a722fe5cb0d7b9652feef1d0a15ed3bf0fe7 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 15 Nov 2016 09:32:15 -0800 Subject: [PATCH] Correct altitude usage for Change Altitude --- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 934774eef..6510bfd77 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -365,7 +365,7 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) { - if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) { + if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) { qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known.")); return; } @@ -397,8 +397,12 @@ void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) { - if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) { - qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known.")); + if (!vehicle->homePositionAvailable()) { + qgcApp()->showMessage(QStringLiteral("Unable to change altitude, home position unknown.")); + return; + } + if (qIsNaN(vehicle->homePosition().altitude())) { + qgcApp()->showMessage(QStringLiteral("Unable to change altitude, home position altitude unknown.")); return; } @@ -413,7 +417,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu cmd.param4 = NAN; cmd.param5 = NAN; cmd.param6 = NAN; - cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel; + cmd.param7 = vehicle->homePosition().altitude() + altitudeRel; cmd.target_system = vehicle->id(); cmd.target_component = vehicle->defaultComponentId(); -- 2.22.0