diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index d88fa06061ef80050015b447e77db7a135dfa680..254ee64583518247855f0ab15c84616a5d085c9d 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -454,6 +454,10 @@ static boolean GPS_light;
// This is current status for the LED lights state machine
// setting this value changes the output of the LEDs
static byte led_mode = NORMAL_LEDS;
+// Blinking indicates GPS status
+static byte copter_leds_GPS_blink = 0;
+// Blinking indicates battery status
+static byte copter_leds_motor_blink = 0;
////////////////////////////////////////////////////////////////////////////////
// GPS variables
@@ -1121,6 +1125,10 @@ static void medium_loop()
USERHOOK_MEDIUMLOOP
#endif
+ #if COPTER_LEDS == ENABLED
+ update_copter_leds();
+ #endif
+
slow_loop();
break;
@@ -1239,10 +1247,6 @@ static void slow_loop()
if(g.radio_tuning > 0)
tuning();
- #if MOTOR_LEDS == 1
- update_motor_leds();
- #endif
-
#if USB_MUX_PIN > 0
check_usb_mux();
#endif
diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde
index 263c954ee816644e3718c87ad406f11859d0c96c..2404e582dc7324635f94112a54fc7c25699ae283 100644
--- a/ArduCopter/leds.pde
+++ b/ArduCopter/leds.pde
@@ -97,39 +97,201 @@ static void clear_leds()
led_mode = NORMAL_LEDS;
}
-#if MOTOR_LEDS == 1
-static void update_motor_leds(void)
+/////////////////////////////////////////////////////////////////////////////////////////////
+// Copter LEDS by Robert Lefebvre
+// Based on the work of U4eake, Bill Sanford, and Max Levine
+// g.copter_leds_mode controls the copter leds function via bitmath
+// Zeroeth bit turns copter leds on and off: 00000001
+// First bit turns GPS function on and off: 00000010
+// Second bit turns Aux function on and off: 00000100
+// This code is written in order to be backwards compatible with the old Motor_LEDS code
+// I hope to include at least some of the Show_LEDS code in the future
+// copter_leds_GPS_blink controls the blinking of the GPS LEDS
+// copter_leds_motor_blink controls the blinking of the motor LEDS
+/////////////////////////////////////////////////////////////////////////////////////////////
+
+
+#if COPTER_LEDS == ENABLED
+static void update_copter_leds(void)
{
- if (motors.armed()){
- if (low_batt == true){
- // blink rear
- static bool blink = false;
-
- if (blink){
- digitalWrite(RE_LED, LED_ON);
- digitalWrite(FR_LED, LED_ON);
- digitalWrite(RI_LED, LED_OFF);
- digitalWrite(LE_LED, LED_OFF);
- }else{
- digitalWrite(RE_LED, LED_OFF);
- digitalWrite(FR_LED, LED_OFF);
- digitalWrite(RI_LED, LED_ON);
- digitalWrite(LE_LED, LED_ON);
+ if (g.copter_leds_mode == 0) { //method of reintializing LED state
+ copter_leds_reset();
+ }
+
+ if ( bitRead(g.copter_leds_mode, 0) ) {
+ if (motors.armed() == true) {
+ if (low_batt == true) {
+ copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds oscillate
+ } else {
+ copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON
}
- blink = !blink;
- }else{
- digitalWrite(RE_LED, LED_ON);
- digitalWrite(FR_LED, LED_ON);
- digitalWrite(RI_LED, LED_ON);
- digitalWrite(LE_LED, LED_ON);
+ } else {
+ copter_leds_blink(); //if motors are not armed, blink motor leds
+ }
+ }
+
+ if ( bitRead(g.copter_leds_mode, 1) ){
+
+ // GPS LED on if we have a fix or Blink GPS LED if we are receiving data
+ // ---------------------------------------------------------------------
+ switch (g_gps->status()){
+
+ case(2):
+ if(home_is_set) {
+ copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set
+ } else {
+ copter_leds_GPS_fast_blink(); //if GPS has fix, but home is not set, blink GPS LED fast
+ }
+ break;
+
+ case(1):
+
+ copter_leds_GPS_slow_blink(); //if GPS has valid reads, but no fix, blink GPS LED slow
+
+ break;
+
+ default:
+ copter_leds_GPS_off(); //if no valid GPS signal, turn GPS LED off
+ break;
+ }
+ }
+
+ if ( bitRead(g.copter_leds_mode, 2) ) {
+ if (200 <= g.rc_7.control_in && g.rc_7.control_in < 400) {
+ copter_leds_aux_on(); //if sub-control of Ch7 is high, turn Aux LED on
+ } else if (g.rc_7.control_in < 200) {
+ copter_leds_aux_off(); //if sub-control of Ch7 is low, turn Aux LED off
+ }
+ }
+
+}
+
+static void copter_leds_reset(void) {
+ digitalWrite(COPTER_LED_1, LED_OFF);
+ digitalWrite(COPTER_LED_2, LED_OFF);
+ digitalWrite(COPTER_LED_3, LED_OFF);
+ digitalWrite(COPTER_LED_4, LED_OFF);
+ digitalWrite(COPTER_LED_5, LED_OFF);
+ digitalWrite(COPTER_LED_6, LED_OFF);
+ digitalWrite(COPTER_LED_7, LED_OFF);
+ digitalWrite(COPTER_LED_8, LED_OFF);
+}
+
+static void copter_leds_on(void) {
+ digitalWrite(COPTER_LED_1, LED_ON);
+ digitalWrite(COPTER_LED_2, LED_ON);
+ digitalWrite(COPTER_LED_3, LED_ON);
+ digitalWrite(COPTER_LED_4, LED_ON);
+ if ( !bitRead(g.copter_leds_mode, 2) ){
+ digitalWrite(COPTER_LED_5, LED_ON);
+ digitalWrite(COPTER_LED_6, LED_ON);
+ }
+ if ( !bitRead(g.copter_leds_mode, 1) ){
+ digitalWrite(COPTER_LED_7, LED_ON);
+ digitalWrite(COPTER_LED_8, LED_ON);
+ }
+}
+
+static void copter_leds_off(void) {
+ digitalWrite(COPTER_LED_1, LED_OFF);
+ digitalWrite(COPTER_LED_2, LED_OFF);
+ digitalWrite(COPTER_LED_3, LED_OFF);
+ digitalWrite(COPTER_LED_4, LED_OFF);
+ if ( !bitRead(g.copter_leds_mode, 2) ){
+ digitalWrite(COPTER_LED_5, LED_OFF);
+ digitalWrite(COPTER_LED_6, LED_OFF);
+ }
+ if ( !bitRead(g.copter_leds_mode, 1) ){
+ digitalWrite(COPTER_LED_7, LED_OFF);
+ digitalWrite(COPTER_LED_8, LED_OFF);
+ }
+}
+
+static void copter_leds_blink(void) {
+ copter_leds_motor_blink++; // this increments once every 1/10 second because it is in the 10hz loop
+ if ( 0 < copter_leds_motor_blink && copter_leds_motor_blink < 6 ){ // when the counter reaches 5 (1/2 sec), then toggle the leds
+ copter_leds_on();
+ }else if (5 < copter_leds_motor_blink && copter_leds_motor_blink < 11){
+ copter_leds_off();
+ }
+ else copter_leds_motor_blink = 0; // start blink cycle again
+}
+
+static void copter_leds_oscillate(void) {
+ copter_leds_motor_blink++; // this increments once every 1/10 second because it is in the 10hz loop
+ if ( 0 < copter_leds_motor_blink && copter_leds_motor_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds
+ digitalWrite(COPTER_LED_1, LED_ON);
+ digitalWrite(COPTER_LED_2, LED_ON);
+ digitalWrite(COPTER_LED_3, LED_OFF);
+ digitalWrite(COPTER_LED_4, LED_OFF);
+ if ( !bitRead(g.copter_leds_mode, 2) ) {
+ digitalWrite(COPTER_LED_5, LED_ON);
+ digitalWrite(COPTER_LED_6, LED_ON);
+ }
+ if ( !bitRead(g.copter_leds_mode, 1) ) {
+ digitalWrite(COPTER_LED_7, LED_OFF);
+ digitalWrite(COPTER_LED_8, LED_OFF);
+ }
+ }else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) {
+ digitalWrite(COPTER_LED_1, LED_OFF);
+ digitalWrite(COPTER_LED_2, LED_OFF);
+ digitalWrite(COPTER_LED_3, LED_ON);
+ digitalWrite(COPTER_LED_4, LED_ON);
+ if ( !bitRead(g.copter_leds_mode, 2) ) {
+ digitalWrite(COPTER_LED_5, LED_OFF);
+ digitalWrite(COPTER_LED_6, LED_OFF);
+ }
+ if ( !bitRead(g.copter_leds_mode, 1) ) {
+ digitalWrite(COPTER_LED_7, LED_ON);
+ digitalWrite(COPTER_LED_8, LED_ON);
}
- }else {
- digitalWrite(RE_LED, LED_OFF);
- digitalWrite(FR_LED, LED_OFF);
- digitalWrite(RI_LED, LED_OFF);
- digitalWrite(LE_LED, LED_OFF);
}
+ else copter_leds_motor_blink = 0; // start blink cycle again
+}
+
+
+
+static void copter_leds_GPS_on(void) {
+ digitalWrite(COPTER_LED_7, LED_ON);
+ digitalWrite(COPTER_LED_8, LED_ON);
}
-#endif
+static void copter_leds_GPS_off(void) {
+ digitalWrite(COPTER_LED_7, LED_OFF);
+ digitalWrite(COPTER_LED_8, LED_OFF);
+}
+
+static void copter_leds_GPS_slow_blink(void) {
+ copter_leds_GPS_blink++; // this increments once every 1/10 second because it is in the 10hz loop
+ if ( 0 < copter_leds_GPS_blink && copter_leds_GPS_blink < 6 ){ // when the counter reaches 5 (1/2 sec), then toggle the leds
+ copter_leds_GPS_off();
+ }else if (5 < copter_leds_GPS_blink && copter_leds_GPS_blink < 11){
+ copter_leds_GPS_on();
+ }
+ else copter_leds_GPS_blink = 0; // start blink cycle again
+}
+
+static void copter_leds_GPS_fast_blink(void) {
+ copter_leds_GPS_blink++; // this increments once every 1/10 second because it is in the 10hz loop
+ if ( 0 < copter_leds_GPS_blink && copter_leds_GPS_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds
+ copter_leds_GPS_off();
+ }else if (2 < copter_leds_GPS_blink && copter_leds_GPS_blink < 5){
+ copter_leds_GPS_on();
+ }
+ else copter_leds_GPS_blink = 0; // start blink cycle again
+}
+
+
+static void copter_leds_aux_off(void)
+{
+ digitalWrite(COPTER_LED_5, LED_OFF);
+ digitalWrite(COPTER_LED_6, LED_OFF);
+}
+
+static void copter_leds_aux_on(void)
+{
+ digitalWrite(COPTER_LED_5, LED_ON);
+ digitalWrite(COPTER_LED_6, LED_ON);
+}
+#endif //COPTER_LEDS