From 1dbe98b566d9401b8f11b773bc43ae70d2261b10 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 20 May 2013 13:03:18 +0900
Subject: [PATCH] Copter: display reason for pre-arm-check failures in GCS

---
 ArduCopter/ArduCopter.pde  |  5 +++--
 ArduCopter/GCS_Mavlink.pde |  2 ++
 ArduCopter/motors.pde      | 31 +++++++++++++++++++++++--------
 3 files changed, 28 insertions(+), 10 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 5919a2b303..2b42833660 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -823,6 +823,7 @@ AC_Fence    fence(&inertial_nav);
 // function definitions to keep compiler from complaining about undeclared functions
 ////////////////////////////////////////////////////////////////////////////////
 void get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate);
+static void pre_arm_checks(bool display_failure);
 
 ////////////////////////////////////////////////////////////////////////////////
 // Top-level logic
@@ -1162,7 +1163,7 @@ static void medium_loop()
         medium_loopCounter = 0;
 
         // Check for motor arming or disarming
-        arm_motors();
+        arm_motors_check();
 
         // agmatthews - USERHOOKS
 #ifdef USERHOOK_MEDIUMLOOP
@@ -1278,7 +1279,7 @@ static void super_slow_loop()
         Log_Write_Current();
 
     // perform pre-arm checks
-    pre_arm_checks();
+    pre_arm_checks(false);
 
     // this function disarms the copter if it has been sitting on the ground for any moment of time greater than 25 seconds
     // but only of the control mode is manual
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index ab0eed5cf1..0682e3b9bb 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -1218,6 +1218,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         case MAV_CMD_COMPONENT_ARM_DISARM:
             if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
                 if (packet.param1 == 1.0f) {
+                    // run pre-arm-checks and display failures
+                    pre_arm_checks(true);
                     if(ap.pre_arm_check) {
                         init_arm_motors();
                     }
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index 7b54b0e544..75ad614124 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -6,8 +6,9 @@
 #define AUTO_TRIM_DELAY 100
 
 
+// arm_motors_check - checks for pilot input to arm or disarm the copter
 // called at 10hz
-static void arm_motors()
+static void arm_motors_check()
 {
     static int16_t arming_counter;
 
@@ -17,11 +18,6 @@ static void arm_motors()
         return;
     }
 
-    // ensure pre-arm checks have been successful
-    if(!ap.pre_arm_check) {
-        return;
-    }
-
     // ensure we are in Stabilize, Acro or TOY mode
     if ((control_mode > ACRO) && ((control_mode != TOY_A) && (control_mode != TOY_M))) {
         arming_counter = 0;
@@ -51,7 +47,14 @@ static void arm_motors()
 
         // arm the motors and configure for flight
         if (arming_counter == ARM_DELAY && !motors.armed()) {
-            init_arm_motors();
+            // run pre-arm-checks and display failures
+            pre_arm_checks(true);
+            if(ap.pre_arm_check) {
+                init_arm_motors();
+            }else{
+                // reset arming counter if pre-arm checks fail
+                arming_counter = 0;
+            }
         }
 
         // arm the motors and configure for flight
@@ -163,7 +166,7 @@ static void init_arm_motors()
 }
 
 // perform pre-arm checks and set ap.pre_arm_check flag
-static void pre_arm_checks()
+static void pre_arm_checks(bool display_failure)
 {
     // exit immediately if we've already successfully performed the pre-arm check
     if( ap.pre_arm_check ) {
@@ -173,22 +176,34 @@ static void pre_arm_checks()
     // pre-arm rc checks a prerequisite
     pre_arm_rc_checks();
     if(!ap.pre_arm_rc_check) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: RC not calibrated"));
+        }
         return;
     }
 
     // check accelerometers have been calibrated
     if(!ins.calibrated()) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not calibrated"));
+        }
         return;
     }
 
     // check the compass is healthy
     if(!compass.healthy) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not healthy"));
+        }
         return;
     }
 
 #if AC_FENCE == ENABLED
     // check fence is initialised
     if(!fence.pre_arm_check()) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: No GPS Lock"));
+        }
         return;
     }
 #endif