From df4246597faa0fc7357089e3784d237bccfa3686 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Tue, 22 May 2012 09:57:38 -0400 Subject: [PATCH] Updates to CopterLEDS Creation of Nav Blink Function. --- ArduCopter/ArduCopter.pde | 2 + ArduCopter/commands_logic.pde | 1 + ArduCopter/control_modes.pde | 2 + ArduCopter/leds.pde | 76 ++++++++++++++++++++++------------- ArduCopter/motors.pde | 4 ++ ArduCopter/sensors.pde | 3 ++ 6 files changed, 61 insertions(+), 27 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 5c891ef6fc..7a17382604 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -471,6 +471,8 @@ static byte led_mode = NORMAL_LEDS; static byte copter_leds_GPS_blink = 0; // Blinking indicates battery status static byte copter_leds_motor_blink = 0; +// Navigation confirmation blinks +static int8_t copter_leds_nav_blink = 0; //////////////////////////////////////////////////////////////////////////////// // GPS variables diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 36ebe85c15..310086ae57 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -513,6 +513,7 @@ static bool verify_nav_wp() sprintf(message,"Reached Command #%i",command_nav_index); gcs_send_text(SEVERITY_LOW,message); wp_verify_byte = 0; + copter_leds_nav_blink = 15; // Cause the CopterLEDs to blink three times to indicate waypoint reached return true; }else{ return false; diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 27c4051427..398235feec 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -141,6 +141,8 @@ static void read_trim_switch() // save command set_cmd_with_index(current_loc, CH7_wp_index); + copter_leds_nav_blink = 10; // Cause the CopterLEDs to blink twice to indicate saved waypoint + // 0 = home // 1 = takeoff // 2 = WP 2 diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index e879dc707e..617ade2cea 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -101,11 +101,13 @@ static void clear_leds() // Copter LEDS by Robert Lefebvre // Based on the work of U4eake, Bill Sanford, Max Levine, and Oliver // g.copter_leds_mode controls the copter leds function via bitmath -// Zeroeth bit turns motor leds on and off: 00000001 -// First bit turns GPS function on and off: 00000010 -// Second bit turns Aux function on and off: 00000100 -// Third bit turns on Beeper (legacy Piezo) function: 00001000 -// Fourth bit toggles between Fast Flash or Oscillate on Low Battery: 000100000 (0) does Fast Flash, (1) does Oscillate +// Zeroeth bit turns motor leds on and off: 00000001 +// First bit turns GPS function on and off: 00000010 +// Second bit turns Aux function on and off: 00000100 +// Third bit turns on Beeper (legacy Piezo) function: 00001000 +// Fourth bit toggles between Fast Flash or Oscillate on Low Battery: 00010000 (0) does Fast Flash, (1) does Oscillate +// Fifth bit causes motor LEDs to Nav Blink: 00100000 +// Sixth bit causes GPS LEDs to Nav Blink: 01000000 // 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 @@ -127,11 +129,17 @@ static void update_copter_leds(void) if (low_batt == true) { if ( bitRead(g.copter_leds_mode, 4 )) { copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds fast blink - }else{ + } else { copter_leds_fast_blink(); //if motors are armed, but battery level is low, motor leds oscillate } - } else { + } else if ( !bitRead(g.copter_leds_mode, 5 ) ) { copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON + } else if ( bitRead(g.copter_leds_mode, 5 ) ) { + if ( copter_leds_nav_blink >0 ) { + copter_leds_slow_blink(); //if nav command was seen, blink LEDs. + } else { + copter_leds_on(); + } } } else { copter_leds_slow_blink(); //if motors are not armed, blink motor leds @@ -146,8 +154,16 @@ static void update_copter_leds(void) case(2): if(home_is_set) { - copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set - } else { + if ( !bitRead(g.copter_leds_mode, 6 ) ) { + copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set + } else if (bitRead(g.copter_leds_mode, 6 ) ) { + if ( copter_leds_nav_blink >0 ) { + copter_leds_GPS_slow_blink(); //if nav command was seen, blink LEDs. + } else { + copter_leds_GPS_on(); + } + } + } else { copter_leds_GPS_fast_blink(); //if GPS has fix, but home is not set, blink GPS LED fast } break; @@ -228,29 +244,32 @@ static void copter_leds_off(void) { } static void copter_leds_slow_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_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_off(); + if ( bitRead(g.copter_leds_mode, 5 ) && !bitRead(g.copter_leds_mode, 6 ) && copter_leds_nav_blink >0 ) { // if blinking is called by the Nav Blinker... + copter_leds_nav_blink--; // decrement the Nav Blink counter + } + }else if (5 < copter_leds_motor_blink && copter_leds_motor_blink < 11){ + copter_leds_on(); } - else copter_leds_motor_blink = 0; // start blink cycle again + else copter_leds_motor_blink = 0; // start blink cycle again } static void copter_leds_fast_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 < 3 ){ // when the counter reaches 3 (1/5 sec), then toggle the leds + 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 copter_leds_on(); }else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5){ copter_leds_off(); } - else copter_leds_motor_blink = 0; // start blink cycle again + 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 + 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 if ( !bitRead(g.copter_leds_mode, 2) ){ digitalWrite(COPTER_LED_1, COPTER_LED_ON); } @@ -289,7 +308,7 @@ static void copter_leds_oscillate(void) { digitalWrite(COPTER_LED_7, COPTER_LED_ON); digitalWrite(COPTER_LED_8, COPTER_LED_ON); } - else copter_leds_motor_blink = 0; // start blink cycle again + else copter_leds_motor_blink = 0; // start blink cycle again } @@ -303,23 +322,26 @@ static void copter_leds_GPS_off(void) { } 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_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(); + if ( bitRead(g.copter_leds_mode, 6 ) && copter_leds_nav_blink >0 ) { // if blinking is called by the Nav Blinker... + copter_leds_nav_blink--; // decrement the Nav Blink counter + } }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 + 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_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 + else copter_leds_GPS_blink = 0; // start blink cycle again } static void copter_leds_aux_off(void){ @@ -344,4 +366,4 @@ void piezo_beep(){ // Note! This command should not be used in tim piezo_off(); } -#endif //COPTER_LEDS +#endif //COPTER_LEDS \ No newline at end of file diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index ee061fa9d9..2186dc5ac3 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -84,11 +84,13 @@ static void init_arm_motors() } motors.armed(true); + #if COPTER_LEDS == ENABLED if ( bitRead(g.copter_leds_mode, 3) ){ piezo_beep(); delay(50); piezo_beep(); } + #endif // Remember Orientation // -------------------- @@ -138,9 +140,11 @@ static void init_disarm_motors() // we are not in the air takeoff_complete = false; + #if COPTER_LEDS == ENABLED if ( bitRead(g.copter_leds_mode, 3) ){ piezo_beep(); } + #endif } /***************************************** diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 662cedc1d4..9c6dab766c 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -135,6 +135,7 @@ static void read_battery(void) if((battery_voltage1 < g.low_voltage) || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity)){ low_battery_event(); + #if COPTER_LEDS == ENABLED if ( bitRead(g.copter_leds_mode, 3) ){ // Only Activate if a battery is connected to avoid alarm on USB only if (battery_voltage1 > 1){ piezo_on(); @@ -142,9 +143,11 @@ static void read_battery(void) piezo_off(); } } + }else if ( bitRead(g.copter_leds_mode, 3) ){ piezo_off(); + #endif // COPTER_LEDS } #endif //BATTERY_EVENT }