From 3aaf1a3367291804a446b174f64cbc6bff782c70 Mon Sep 17 00:00:00 2001
From: rodneygrainger <op.rodney@gmail.com>
Date: Mon, 7 Jul 2014 20:47:23 +1200
Subject: [PATCH] OP-1391 - Fix to prevent arming if unsafe due to current
 Thrust Mode (Altitude Hold/Vario)

---
 flight/modules/ManualControl/armhandler.c | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c
index 1615ac91b..7dbf79336 100644
--- a/flight/modules/ManualControl/armhandler.c
+++ b/flight/modules/ManualControl/armhandler.c
@@ -35,6 +35,7 @@
 #include <accessorydesired.h>
 #include <flightstatus.h>
 #include <flightmodesettings.h>
+#include <stabilizationdesired.h>
 
 // Private constants
 #define ARMED_THRESHOLD 0.50f
@@ -267,6 +268,8 @@ static bool okToArm(void)
         }
     }
 
+    StabilizationDesiredStabilizationModeData stabDesired;
+
     uint8_t flightMode;
     FlightStatusFlightModeGet(&flightMode);
     switch (flightMode) {
@@ -277,8 +280,14 @@ static bool okToArm(void)
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
-        return true;
-
+        // Prevent arming if unsafe due to the current Thrust Mode
+        StabilizationDesiredStabilizationModeGet(&stabDesired);
+        if (stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD ||
+            stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) {
+            return false;
+        } else {
+            return true;
+        }
         break;
 
     default: