Commit 004bda50 authored by Don Gagne's avatar Don Gagne

Correct guided mode AMSL versus Relative handling

parent 099fe046
...@@ -120,16 +120,11 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle) ...@@ -120,16 +120,11 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
{ {
if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
qgcApp()->showMessage(QStringLiteral("Unable to takeoff, vehicle position not known."));
return;
}
vehicle->sendMavCommand(vehicle->defaultComponentId(), vehicle->sendMavCommand(vehicle->defaultComponentId(),
MAV_CMD_NAV_TAKEOFF, MAV_CMD_NAV_TAKEOFF,
true, // show error true, // show error
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
vehicle->altitudeAMSL()->rawValue().toFloat() + altitudeRel); // AMSL meters altitudeRel);
} }
void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
......
...@@ -437,7 +437,7 @@ Item { ...@@ -437,7 +437,7 @@ Item {
break; break;
case confirmChangeAlt: case confirmChangeAlt:
altitudeSlider.visible = true altitudeSlider.visible = true
altitudeSlider.setInitialValueAppSettingsDistanceUnits(_activeVehicle.altitudeAMSL.value) altitudeSlider.setInitialValueAppSettingsDistanceUnits(_activeVehicle.altitudeRelative.value)
guidedModeConfirm.confirmText = qsTr("change altitude") guidedModeConfirm.confirmText = qsTr("change altitude")
break; break;
case confirmGoTo: case confirmGoTo:
......
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