From d2bd818b2dc3e39a8eef91ec67b8b0679ac87980 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 17 Apr 2013 21:25:32 +0900
Subject: [PATCH] Copter: bug fix for auto_armed logic

Zero throttle when switching into an auto flight mode would cause the
motors to stop.
---
 ArduCopter/ArduCopter.pde  |  7 +++++--
 ArduCopter/GCS_Mavlink.pde |  2 +-
 ArduCopter/motors.pde      |  3 ---
 ArduCopter/system.pde      | 28 +++++++++++++++++++++++-----
 ArduCopter/test.pde        |  2 +-
 5 files changed, 30 insertions(+), 12 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 2abecb4520..89cc9e97d2 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -365,7 +365,7 @@ static union {
 
         uint8_t low_battery        : 1; // 5    // Used to track if the battery is low - LED output flashes when the batt is low
         uint8_t armed              : 1; // 6
-        uint8_t auto_armed         : 1; // 7
+        uint8_t auto_armed         : 1; // 7    // stops auto missions from beginning until throttle is raised
 
         uint8_t failsafe_radio     : 1; // 8    // A status flag for the radio failsafe
         uint8_t failsafe_batt      : 1; // 9    // A status flag for the battery failsafe
@@ -1165,6 +1165,9 @@ static void fifty_hz_loop()
     edf_toy();
 #endif
 
+    // check auto_armed status
+    update_auto_armed();
+
 #ifdef USERHOOK_50HZLOOP
     USERHOOK_50HZLOOP
 #endif
@@ -1967,7 +1970,7 @@ void update_throttle_mode(void)
 
     case THROTTLE_AUTO:
         // auto pilot altitude controller with target altitude held in wp_nav.get_desired_alt()
-        if(motors.auto_armed() == true) {
+        if(ap.auto_armed) {
             get_throttle_althold_with_slew(wp_nav.get_desired_alt(), g.auto_velocity_z_min, g.auto_velocity_z_max);
             set_target_alt_for_reporting(wp_nav.get_desired_alt()); // To-Do: return get_destination_alt if we are flying to a waypoint
         }
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 38571d0760..c6a84edd5f 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -327,7 +327,7 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
 #else
  #if X_PLANE == ENABLED
     /* update by JLN for X-Plane HIL */
-    if(motors.armed() && motors.auto_armed()) {
+    if(motors.armed() && ap.auto_armed) {
         mavlink_msg_rc_channels_scaled_send(
             chan,
             millis(),
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index 330f25a582..d2ce56bb11 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -176,9 +176,6 @@ static void init_disarm_motors()
     motors.armed(false);
     set_armed(false);
 
-    motors.auto_armed(false);
-    set_auto_armed(false);
-
     compass.save_offsets();
 
     g.throttle_cruise.save();
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 78bc7b9474..09aa0a766a 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -366,11 +366,6 @@ static void set_mode(uint8_t mode)
     control_mode 	= mode;
     control_mode    = constrain(control_mode, 0, NUM_MODES - 1);
 
-    // used to stop fly_aways
-    // set to false if we have low throttle
-    motors.auto_armed(g.rc_3.control_in > 0 || ap.failsafe_radio);
-    set_auto_armed(g.rc_3.control_in > 0 || ap.failsafe_radio);
-
     // if we change modes, we must clear landed flag
     set_land_complete(false);
 
@@ -534,6 +529,29 @@ init_simple_bearing()
     }
 }
 
+// update_auto_armed - update status of auto_armed flag
+static void update_auto_armed()
+{
+    // disarm checks
+    if(ap.auto_armed){
+        // if motors are disarmed, auto_armed should also be false
+        if(!motors.armed()) {
+            set_auto_armed(false);
+            return;
+        }
+        // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
+        if(control_mode <= ACRO && g.rc_3.control_in == 0 && !ap.failsafe_radio) {
+            set_auto_armed(false);
+        }
+    }else{
+        // arm checks
+        // if motors are armed and throttle is above zero auto_armed should be true
+        if(motors.armed() && g.rc_3.control_in != 0) {
+            set_auto_armed(true);
+        }
+    }
+}
+
 /*
  *  map from a 8 bit EEPROM baud rate to a real baud rate
  */
diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde
index 68539ecec4..fe97c107fe 100644
--- a/ArduCopter/test.pde
+++ b/ArduCopter/test.pde
@@ -348,7 +348,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
  *       cliSerial->printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP());
  *       cliSerial->printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
  *
- *       motors.auto_armed(false);
+ *       set_auto_armed(false);
  *       motors.armed(true);
  *
  *       while(1){