From a6420f4ad6c74fa38633791061d0ed3af6a42a9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 May 2016 12:39:21 +0200 Subject: [PATCH] PX4: Fix guided mode interaction --- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 571bd9c6e..272afea8c 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -258,11 +258,6 @@ QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile) void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle) { - // kick it into hold mode - vehicle->setFlightMode(pauseFlightMode); - - QGC::SLEEP::msleep(200); - // then tell it to loiter at the current position mavlink_message_t msg; mavlink_command_long_t cmd; @@ -270,7 +265,7 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle) cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; cmd.confirmation = 0; cmd.param1 = -1.0f; - cmd.param2 = 0.0; + cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; cmd.param3 = 0.0f; cmd.param4 = NAN; cmd.param5 = NAN; @@ -324,7 +319,7 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); vehicle->sendMessage(msg); - QGC::SLEEP::msleep(200); + QGC::SLEEP::msleep(300); // trigger take off vehicle->setFlightMode(takeoffFlightMode); -- 2.22.0