From 064a34c667853ec56b4dcc913b081ea1c45f7979 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 9 May 2016 22:55:17 +0200
Subject: [PATCH] Firmware plugin: Sequence guided mode correctly, widen modes
 in which guided support should be available

---
 src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 26 ++++++++++-----------
 1 file changed, 12 insertions(+), 14 deletions(-)

diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
index 712fe709c..571bd9c6e 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
@@ -117,8 +117,6 @@ QStringList PX4FirmwarePlugin::flightModes(void)
 {
     QStringList flightModes;
 
-    // FIXME: fixed-wing/multi-rotor differences?
-
     for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
         const struct Modes2Name* pModes2Name = &rgModes2Name[i];
 
@@ -138,8 +136,6 @@ QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) c
         union px4_custom_mode px4_mode;
         px4_mode.data = custom_mode;
 
-        // FIXME: fixed-wing/multi-rotor differences?
-
         bool found = false;
         for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
             const struct Modes2Name* pModes2Name = &rgModes2Name[i];
@@ -265,8 +261,9 @@ 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
-    // above the takeoff (home) position
     mavlink_message_t msg;
     mavlink_command_long_t cmd;
 
@@ -306,19 +303,16 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
         return;
     }
 
-    // tell the system first to take off in its internal,
-    // airframe specific takeoff action
-    vehicle->setFlightMode(takeoffFlightMode);
+    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
 
-    // then tell it to loiter at the user-selected location
-    // above the takeoff (home) position
+    // Set destination altitude
     mavlink_message_t msg;
     mavlink_command_long_t cmd;
 
     cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
     cmd.confirmation = 0;
     cmd.param1 = -1.0f;
-    cmd.param2 = 0.0;
+    cmd.param2 = 0.0f;
     cmd.param3 = 0.0f;
     cmd.param4 = NAN;
     cmd.param5 = NAN;
@@ -327,10 +321,13 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
     cmd.target_system = vehicle->id();
     cmd.target_component = 0;
 
-    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
     mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
-
     vehicle->sendMessage(msg);
+
+    QGC::SLEEP::msleep(200);
+
+    // trigger take off
+    vehicle->setFlightMode(takeoffFlightMode);
 }
 
 void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
@@ -401,5 +398,6 @@ void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
 bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
 {
     // Not supported by generic vehicle
-    return (vehicle->flightMode() == pauseFlightMode);
+    return (vehicle->flightMode() == pauseFlightMode || vehicle->flightMode() == takeoffFlightMode
+            || vehicle->flightMode() == landingFlightMode);
 }
-- 
2.22.0