From db893d288ba7decb0bdd888591baa3047ef6d8bb Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 4 Jul 2013 16:22:34 -1000
Subject: [PATCH] Copter: double flash arming light when pre-arm checks fail

Hardly my finest work but it's temporary because we're going to replace
all the led, buzzer etc with a library called AP_Notify in the near
future.
---
 ArduCopter/ArduCopter.pde |  7 ++--
 ArduCopter/leds.pde       | 81 +++++++++++++++++++++++++++++++++------
 ArduCopter/motors.pde     |  7 ++--
 3 files changed, 75 insertions(+), 20 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 0c2db4b85f..d72c16c19c 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -387,7 +387,7 @@ static union {
 
 static struct AP_System{
     uint8_t GPS_light               : 1; // 0   // Solid indicates we have full 3D lock and can navigate, flash = read
-    uint8_t motor_light             : 1; // 1   // Solid indicates Armed state
+    uint8_t arming_light            : 1; // 1   // Solid indicates armed state, flashing is disarmed, double flashing is disarmed and failing pre-arm checks
     uint8_t new_radio_frame         : 1; // 2   // Set true if we have new PWM data to act on from the Radio
     uint8_t CH7_flag                : 1; // 3   // true if ch7 aux switch is high
     uint8_t CH8_flag                : 1; // 4   // true if ch8 aux switch is high
@@ -1175,6 +1175,8 @@ static void medium_loop()
         USERHOOK_MEDIUMLOOP
 #endif
 
+        // update board leds
+        update_board_leds();
 #if COPTER_LEDS == ENABLED
         update_copter_leds();
 #endif
@@ -1254,9 +1256,6 @@ static void slow_loop()
         slow_loopCounter = 0;
         update_events();
 
-        // blink if we are armed
-        update_lights();
-
         if(g.radio_tuning > 0)
             tuning();
 
diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde
index b1e19fa34f..f7885e25c3 100644
--- a/ArduCopter/leds.pde
+++ b/ArduCopter/leds.pde
@@ -1,15 +1,27 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-static void update_lights()
+// update_board_leds - updates leds on board
+// should be called at 10hz
+static void update_board_leds()
 {
+    // we need to slow down the calls to the GPS and dancing lights to 3.33hz
+    static uint8_t counter = 0;
+    if(++counter >= 3){
+        counter = 0;
+    }
+
     switch(led_mode) {
     case NORMAL_LEDS:
-        update_motor_light();
-        update_GPS_light();
+        update_arming_light();
+        if (counter == 0) {
+            update_GPS_light();
+        }
         break;
 
     case SAVE_TRIM_LEDS:
-        dancing_light();
+        if (counter == 0) {
+            dancing_light();
+        }
         break;
     }
 }
@@ -47,20 +59,64 @@ static void update_GPS_light(void)
     }
 }
 
-static void update_motor_light(void)
+static void update_arming_light(void)
 {
-    if(motors.armed() == false) {
-        ap_system.motor_light = !ap_system.motor_light;
+    // counter to control state
+    static int8_t counter = 0;
+    counter++;
 
-        // blink
-        if(ap_system.motor_light) {
+    // disarmed
+    if(!motors.armed()) {
+        if(!ap.pre_arm_check) {
+            // failed pre-arm checks so double flash
+            switch(counter) {
+                case 0:
+                    ap_system.arming_light = true;
+                    break;
+                case 1:
+                    ap_system.arming_light = false;
+                    break;
+                case 2:
+                    ap_system.arming_light = true;
+                    break;
+                case 3:
+                case 4:
+                    ap_system.arming_light = false;
+                    break;
+                default:
+                    // reset counter to restart the sequence
+                    counter = -1;
+                    break;
+            }
+        }else{
+            // passed pre-arm checks so slower single flash
+            switch(counter) {
+                case 0:
+                case 1:
+                case 2:
+                    ap_system.arming_light = true;
+                    break;
+                case 3:
+                case 4:
+                case 5:
+                    ap_system.arming_light = false;
+                    break;
+                default:
+                    // reset counter to restart the sequence
+                    counter = -1;
+                    break;
+            }
+        }
+        // set arming led from arming_light flag
+        if(ap_system.arming_light) {
             digitalWrite(A_LED_PIN, LED_ON);
         }else{
             digitalWrite(A_LED_PIN, LED_OFF);
         }
     }else{
-        if(!ap_system.motor_light) {
-            ap_system.motor_light = true;
+        // armed
+        if(!ap_system.arming_light) {
+            ap_system.arming_light = true;
             digitalWrite(A_LED_PIN, LED_ON);
         }
     }
@@ -91,12 +147,13 @@ static void dancing_light()
         break;
     }
 }
+
 static void clear_leds()
 {
     digitalWrite(A_LED_PIN, LED_OFF);
     digitalWrite(B_LED_PIN, LED_OFF);
     digitalWrite(C_LED_PIN, LED_OFF);
-    ap_system.motor_light = false;
+    ap_system.arming_light = false;
     led_mode = NORMAL_LEDS;
 }
 
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index a781885a9c..07aab627fa 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -159,10 +159,6 @@ static void init_arm_motors()
     init_barometer();
 #endif
 
-    // temp hack
-    ap_system.motor_light = true;
-    digitalWrite(A_LED_PIN, LED_ON);
-
     // go back to normal AHRS gains
     ahrs.set_fast_gains(false);
 #if SECONDARY_DMP_ENABLED == ENABLED
@@ -175,6 +171,9 @@ static void init_arm_motors()
     // set hover throttle
     motors.set_mid_throttle(g.throttle_mid);
 
+    // update leds on board
+    update_arming_light();
+
 #if COPTER_LEDS == ENABLED
     piezo_beep_twice();
 #endif