From 99b77f19d863bdc92d9b18725ca5331faef4fb41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 May 2016 14:00:08 +0200 Subject: [PATCH] Fix takeoff logic --- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 272afea8c..8b19a6e1c 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -304,7 +304,7 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) mavlink_message_t msg; mavlink_command_long_t cmd; - cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; + cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF; cmd.confirmation = 0; cmd.param1 = -1.0f; cmd.param2 = 0.0f; @@ -318,11 +318,6 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); vehicle->sendMessage(msg); - - QGC::SLEEP::msleep(300); - - // trigger take off - vehicle->setFlightMode(takeoffFlightMode); } void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) -- 2.22.0