Copter: move all arm check logic into arm_checks

ArduCopter/GCS_Mavlink.pde

@@ -1146,14 +1146,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             if (packet.param1 == 1.0f) {
                 // run pre_arm_checks and arm_checks and display failures
                 pre_arm_checks(true);
-                if(ap.pre_arm_check && arm_checks(true)) {
+                if(ap.pre_arm_check && arm_checks(true, true)) {
                     init_arm_motors();
                     result = MAV_RESULT_ACCEPTED;
                 }else{
                     AP_Notify::flags.arming_failed = true;  // init_arm_motors function will reset flag back to false
                     result = MAV_RESULT_UNSUPPORTED;
                 }
-            } else if (packet.param1 == 0.0f)  {
+            } else if (packet.param1 == 0.0f && (manual_flight_mode(control_mode) || ap.land_complete))  {
                 init_disarm_motors();
                 result = MAV_RESULT_ACCEPTED;
             } else {

ArduCopter/motors.pde

@@ -12,7 +12,6 @@ static uint8_t auto_disarming_counter;
 static void arm_motors_check()
 {
     static int16_t arming_counter;
-    bool allow_arming = false;
 
     // ensure throttle is down
     if (g.rc_3.control_in > 0) {
@@ -20,30 +19,6 @@ static void arm_motors_check()
         return;
     }
 
-    // allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and DRIFT
-    if (manual_flight_mode(control_mode)) {
-        allow_arming = true;
-    }
-
-    // allow arming/disarming in Loiter and AltHold if landed
-    if (ap.land_complete && (control_mode == LOITER || control_mode == ALT_HOLD || control_mode == POSHOLD || control_mode == AUTOTUNE)) {
-        allow_arming = true;
-    }
-
-    // kick out other flight modes
-    if (!allow_arming) {
-        arming_counter = 0;
-        return;
-    }
-
-    #if FRAME_CONFIG == HELI_FRAME
-    // heli specific arming check
-    if (!motors.allow_arming()){
-        arming_counter = 0;
-        return;
-    }
-    #endif  // HELI_FRAME
-
     int16_t tmp = g.rc_4.control_in;
 
     // full right
@@ -58,7 +33,7 @@ static void arm_motors_check()
         if (arming_counter == ARM_DELAY && !motors.armed()) {
             // run pre-arm-checks and display failures
             pre_arm_checks(true);
-            if(ap.pre_arm_check && arm_checks(true)) {
+            if(ap.pre_arm_check && arm_checks(true,false)) {
                 init_arm_motors();
             }else{
                 // reset arming counter if pre-arm checks fail
@@ -76,6 +51,10 @@ static void arm_motors_check()
 
     // full left
     }else if (tmp < -4000) {
+        if (!manual_flight_mode(control_mode) && !ap.land_complete) {
+            arming_counter = 0;
+            return;
+        }
 
         // increase the counter to a maximum of 1 beyond the disarm delay
         if( arming_counter <= DISARM_DELAY ) {
@@ -525,8 +504,27 @@ static bool pre_arm_gps_checks(bool display_failure)
 
 // arm_checks - perform final checks before arming
 // always called just before arming.  Return true if ok to arm
-static bool arm_checks(bool display_failure)
+static bool arm_checks(bool display_failure, bool request_from_gcs)
 {
+    // always check if the current mode allows arming
+    if (!mode_allows_arming(control_mode) || (!request_from_gcs && control_mode == GUIDED)) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Mode not armable"));
+        }
+        return false;
+    }
+
+    // always check if rotor is spinning on heli
+    #if FRAME_CONFIG == HELI_FRAME
+    // heli specific arming check
+    if (!motors.allow_arming()){
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Rotor not spinning"));
+        }
+        return false;
+    }
+    #endif  // HELI_FRAME
+
     // succeed if arming checks are disabled
     if (g.arming_check == ARMING_CHECK_NONE) {
         return true;