From 7cf225582268781ffe51ffaac7b6bfbfb4a0a5d0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 1 Apr 2013 13:10:12 +0900 Subject: [PATCH] Copter LEDs: replace bitRead with bitmask Consolidate all checks of led_mode to leds.pde Add #defines for bitmasks comparisons Some formatting changes --- ArduCopter/compat.h | 3 - ArduCopter/events.pde | 5 +- ArduCopter/leds.pde | 169 ++++++++++++++++++++++++++---------------- ArduCopter/motors.pde | 10 +-- ArduCopter/system.pde | 15 +--- 5 files changed, 111 insertions(+), 91 deletions(-) diff --git a/ArduCopter/compat.h b/ArduCopter/compat.h index ddf5f7d176..451e28c2c9 100644 --- a/ArduCopter/compat.h +++ b/ArduCopter/compat.h @@ -11,8 +11,5 @@ /* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */ static void run_cli(AP_HAL::UARTDriver *port); -// XXX AP_HAL tofix - what is bitRead? temporarily disable. -#define bitRead(a,b) false - #endif // __COMPAT_H__ diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index 6512b3c42b..ceba714837 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -98,9 +98,8 @@ static void low_battery_event(void) Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); #if COPTER_LEDS == ENABLED - if ( bitRead(g.copter_leds_mode, 3) ) { // Only Activate if a battery is connected to avoid alarm on USB only - piezo_on(); - } + // Only Activate if a battery is connected to avoid alarm on USB only + piezo_on(); #endif // COPTER_LEDS } diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index 53fb235dc6..77ab50c3ff 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -119,29 +119,55 @@ static void clear_leds() // Piezo Enables Tone on reaching low battery or current alert ///////////////////////////////////////////////////////////////////////////////////////////// +#define COPTER_LEDS_BITMASK_ENABLED 0x01 // bit #0 +#define COPTER_LEDS_BITMASK_GPS 0x02 // bit #1 +#define COPTER_LEDS_BITMASK_AUX 0x04 // bit #2 +#define COPTER_LEDS_BITMASK_BEEPER 0x08 // bit #3 +#define COPTER_LEDS_BITMASK_BATT_OSCILLATE 0x10 // bit #4 +#define COPTER_LEDS_BITMASK_MOTOR_NAV_BLINK 0x20 // bit #5 +#define COPTER_LEDS_BITMASK_GPS_NAV_BLINK 0x40 // bit #6 #if COPTER_LEDS == ENABLED +static void copter_leds_init(void) +{ + pinMode(COPTER_LED_1, OUTPUT); //Motor LED + pinMode(COPTER_LED_2, OUTPUT); //Motor LED + pinMode(COPTER_LED_3, OUTPUT); //Motor LED + pinMode(COPTER_LED_4, OUTPUT); //Motor LED + pinMode(COPTER_LED_5, OUTPUT); //Motor or Aux LED + pinMode(COPTER_LED_6, OUTPUT); //Motor or Aux LED + pinMode(COPTER_LED_7, OUTPUT); //Motor or GPS LED + pinMode(COPTER_LED_8, OUTPUT); //Motor or GPS LED + + if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) { + piezo_beep(); + } +} + static void update_copter_leds(void) { if (g.copter_leds_mode == 0) { copter_leds_reset(); //method of reintializing LED state } - if ( bitRead(g.copter_leds_mode, 0) ) { - if (motors.armed() == true) { - if (ap.low_battery == true) { - if ( bitRead(g.copter_leds_mode, 4 )) { + // motor leds control + if (g.copter_leds_mode & COPTER_LEDS_BITMASK_ENABLED) { + if (motors.armed()) { + if (ap.low_battery) { + if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BATT_OSCILLATE) { copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds fast blink } else { copter_leds_fast_blink(); //if motors are armed, but battery level is low, motor leds oscillate } - } 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 { + if (g.copter_leds_mode & COPTER_LEDS_BITMASK_MOTOR_NAV_BLINK) { + if (copter_leds_nav_blink > 0) { + copter_leds_slow_blink(); //if nav command was seen, blink LEDs. + } else { + copter_leds_on(); + } } else { - copter_leds_on(); + copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON } } } else { @@ -149,48 +175,48 @@ static void update_copter_leds(void) } } - if ( bitRead(g.copter_leds_mode, 1) ) { + // GPS led control + if (g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS) { // GPS LED on if we have a fix or Blink GPS LED if we are receiving data // --------------------------------------------------------------------- switch (g_gps->status()) { - case (2): + case GPS::NO_GPS: + copter_leds_GPS_off(); //if no valid GPS signal, turn GPS LED off + break; + + case GPS::NO_FIX: + copter_leds_GPS_slow_blink(); //if GPS has valid reads, but no fix, blink GPS LED slow + break; + + case GPS::GPS_OK_FIX_2D: + case GPS::GPS_OK_FIX_3D: if(ap.home_is_set) { - 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 ) { + if (g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS_NAV_BLINK) { + 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_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) { + // AUX led control + if (g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX) { + if (ap_system.CH7_flag) { copter_leds_aux_on(); //if sub-control of Ch7 is high, turn Aux LED on - } else if (g.rc_7.control_in < 200) { + } else { copter_leds_aux_off(); //if sub-control of Ch7 is low, turn Aux LED off } } - } static void copter_leds_reset(void) { @@ -205,17 +231,17 @@ static void copter_leds_reset(void) { } static void copter_leds_on(void) { - if ( !bitRead(g.copter_leds_mode, 2) ) { + if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) { digitalWriteFast(COPTER_LED_1, COPTER_LED_ON); } #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 - if ( !bitRead(g.copter_leds_mode, 3) ) { + if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) { digitalWriteFast(COPTER_LED_2, COPTER_LED_ON); } #else digitalWriteFast(COPTER_LED_2, COPTER_LED_ON); #endif - if ( !bitRead(g.copter_leds_mode, 1) ) { + if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS) ) { digitalWriteFast(COPTER_LED_3, COPTER_LED_ON); } digitalWriteFast(COPTER_LED_4, COPTER_LED_ON); @@ -226,17 +252,17 @@ static void copter_leds_on(void) { } static void copter_leds_off(void) { - if ( !bitRead(g.copter_leds_mode, 2) ) { + if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) { digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF); } #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 - if ( !bitRead(g.copter_leds_mode, 3) ) { + if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) { digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF); } #else digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF); #endif - if ( !bitRead(g.copter_leds_mode, 1) ) { + if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS)) { digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF); } digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF); @@ -247,43 +273,47 @@ 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_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 + + // if blinking is called by the Nav Blinker decrement the Nav Blink counter + if ((g.copter_leds_mode & COPTER_LEDS_BITMASK_MOTOR_NAV_BLINK) && !(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS_NAV_BLINK) && copter_leds_nav_blink >0 ) { + copter_leds_nav_blink--; } }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 +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_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 - if ( !bitRead(g.copter_leds_mode, 2) ) { + 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 ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) { digitalWriteFast(COPTER_LED_1, COPTER_LED_ON); } #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 - if ( !bitRead(g.copter_leds_mode, 3) ) { + if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) { digitalWriteFast(COPTER_LED_2, COPTER_LED_ON); } #else digitalWriteFast(COPTER_LED_2, COPTER_LED_ON); #endif - if ( !bitRead(g.copter_leds_mode, 1) ) { + if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS)) { digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF); } digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF); @@ -292,17 +322,17 @@ static void copter_leds_oscillate(void) { digitalWriteFast(COPTER_LED_7, COPTER_LED_OFF); digitalWriteFast(COPTER_LED_8, COPTER_LED_OFF); }else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) { - if ( !bitRead(g.copter_leds_mode, 2) ) { + if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) { digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF); } #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 - if ( !bitRead(g.copter_leds_mode, 3) ) { + if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) { digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF); } #else digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF); #endif - if ( !bitRead(g.copter_leds_mode, 1) ) { + if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS) ) { digitalWriteFast(COPTER_LED_3, COPTER_LED_ON); } digitalWriteFast(COPTER_LED_4, COPTER_LED_ON); @@ -310,12 +340,11 @@ static void copter_leds_oscillate(void) { digitalWriteFast(COPTER_LED_6, COPTER_LED_OFF); digitalWriteFast(COPTER_LED_7, COPTER_LED_ON); digitalWriteFast(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 } - - static void copter_leds_GPS_on(void) { digitalWriteFast(COPTER_LED_3, COPTER_LED_ON); } @@ -328,7 +357,7 @@ 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(); - if ( bitRead(g.copter_leds_mode, 6 ) && copter_leds_nav_blink >0 ) { // if blinking is called by the Nav Blinker... + if ((g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS_NAV_BLINK) && 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) { @@ -356,17 +385,31 @@ static void copter_leds_aux_on(void){ } void piezo_on(){ - digitalWriteFast(PIEZO_PIN,HIGH); + if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) { + digitalWriteFast(PIEZO_PIN,HIGH); + } } void piezo_off(){ - digitalWriteFast(PIEZO_PIN,LOW); + if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) { + digitalWriteFast(PIEZO_PIN,LOW); + } } void piezo_beep(){ // Note! This command should not be used in time sensitive loops - piezo_on(); - delay(100); - piezo_off(); + if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) { + piezo_on(); + delay(100); + piezo_off(); + } +} + +void piezo_beep_twice(){ // Note! This command should not be used in time sensitive loops + if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) { + piezo_beep(); + delay(50); + piezo_beep(); + } } #endif //COPTER_LEDS diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index eb4e68fb9b..330f25a582 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -120,11 +120,7 @@ static void init_arm_motors() } #if COPTER_LEDS == ENABLED - if ( bitRead(g.copter_leds_mode, 3) ) { - piezo_beep(); - delay(50); - piezo_beep(); - } + piezo_beep_twice(); #endif // Remember Orientation @@ -191,9 +187,7 @@ static void init_disarm_motors() set_takeoff_complete(false); #if COPTER_LEDS == ENABLED - if ( bitRead(g.copter_leds_mode, 3) ) { - piezo_beep(); - } + piezo_beep(); #endif // setup fast AHRS gains to get right attitude diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index e927a41f94..7ad2625efa 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -136,22 +136,9 @@ static void init_ardupilot() relay.init(); #if COPTER_LEDS == ENABLED - pinMode(COPTER_LED_1, OUTPUT); //Motor LED - pinMode(COPTER_LED_2, OUTPUT); //Motor LED - pinMode(COPTER_LED_3, OUTPUT); //Motor LED - pinMode(COPTER_LED_4, OUTPUT); //Motor LED - pinMode(COPTER_LED_5, OUTPUT); //Motor or Aux LED - pinMode(COPTER_LED_6, OUTPUT); //Motor or Aux LED - pinMode(COPTER_LED_7, OUTPUT); //Motor or GPS LED - pinMode(COPTER_LED_8, OUTPUT); //Motor or GPS LED - - if ( !bitRead(g.copter_leds_mode, 3) ) { - piezo_beep(); - } - + copter_leds_init(); #endif - // load parameters from EEPROM load_parameters();