From fee9f7e07179566e98fc0a136842aba3011b2f1b Mon Sep 17 00:00:00 2001
From: Laurent Lalanne <f5soh@free.fr>
Date: Sat, 18 Jun 2016 19:10:41 +0200
Subject: [PATCH] LP-339 Only allow switching to AutoTakeOff if disarmed state

---
 flight/modules/ManualControl/manualcontrol.c | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c
index ec9a300b7..af5195bbf 100644
--- a/flight/modules/ManualControl/manualcontrol.c
+++ b/flight/modules/ManualControl/manualcontrol.c
@@ -230,8 +230,9 @@ static void manualControlTask(void)
     FlightModeSettingsData modeSettings;
     FlightModeSettingsGet(&modeSettings);
 
+    static uint8_t lastPosition      = 0;
     uint8_t position = cmd.FlightModeSwitchPosition;
-    uint8_t newMode  = flightStatus.FlightMode;
+    uint8_t newMode = flightStatus.FlightMode;
     uint8_t newAlwaysStabilized      = flightStatus.AlwaysStabilizeWhenArmed;
     uint8_t newFlightModeAssist      = flightStatus.FlightModeAssist;
     uint8_t newAssistedControlState  = flightStatus.AssistedControlState;
@@ -240,6 +241,13 @@ static void manualControlTask(void)
         newMode = modeSettings.FlightModePosition[position];
     }
 
+    // Ignore change to AutoTakeOff and keep last flight mode position
+    // if vehicle is already armed and maybe in air...
+    if ((newMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF) && flightStatus.Armed) {
+        newMode  = flightStatus.FlightMode;
+        position = lastPosition;
+    }
+
     // if a mode change occurs we default the assist mode and states here
     // to avoid having to add it to all of the below modes that are
     // otherwise unrelated
@@ -507,6 +515,7 @@ static void manualControlTask(void)
         flightStatus.AssistedThrottleState    = newAssistedThrottleState;
         FlightStatusSet(&flightStatus);
         newinit = true;
+        lastPosition = position;
     }
     if (handler->handler) {
         handler->handler(newinit);