From a9349a6e87ab88d29e38f0880572fff96df42b1e Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Tue, 15 May 2012 12:00:21 -0400 Subject: [PATCH] More CopterLEDS changes. Merged CopterLEDS and Piezo functions to eliminate pin conflict on APM2. Created new Parameter bit to turn piezo function on and off from MP. Moved GPS and Aux pin assignments to allow commonality between APM1 and 2. Set LED_Mode parameter default to 9 in order to make CopterLEDS completely backwards compatible with old Motor LEDS and Piezo code. Legacy users should see no difference. --- ArduCopter/Parameters.h | 2 +- ArduCopter/config.h | 33 ++------ ArduCopter/events.pde | 21 ----- ArduCopter/leds.pde | 173 +++++++++++++++++++++++++--------------- ArduCopter/motors.pde | 15 ++-- ArduCopter/sensors.pde | 21 +++-- ArduCopter/system.pde | 11 +-- 7 files changed, 140 insertions(+), 136 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index d85f2e4afc..a84e2f1546 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -338,7 +338,7 @@ public: rtl_approach_alt (0.0), retro_loiter (RETRO_LOITER_MODE), axis_enabled (AXIS_LOCK_ENABLED), - copter_leds_mode (0), + copter_leds_mode (9), waypoint_mode (0), command_total (0), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index e77fca01f1..a2b7689f82 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -152,7 +152,6 @@ //////////////////////////////////////////////////////////////////////////////// // CopterLEDs -// The CopterLEDs output conflicts with APM2 Piezo Beeper output on AN5 // #ifndef COPTER_LEDS @@ -163,19 +162,19 @@ #define COPTER_LED_OFF LOW #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 -#define COPTER_LED_1 AN4 // Motor LED -#define COPTER_LED_2 AN5 // Motor or Aux LED -#define COPTER_LED_3 AN6 // Motor LED -#define COPTER_LED_4 AN7 // Motor or GPS LED +#define COPTER_LED_1 AN4 // Motor or Aux LED +#define COPTER_LED_2 AN5 // Motor LED or Beeper +#define COPTER_LED_3 AN6 // Motor or GPS LED +#define COPTER_LED_4 AN7 // Motor LED #define COPTER_LED_5 AN8 // Motor LED #define COPTER_LED_6 AN9 // Motor LED #define COPTER_LED_7 AN10 // Motor LED #define COPTER_LED_8 AN11 // Motor LED #elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 -#define COPTER_LED_1 AN8 // Motor LED -#define COPTER_LED_2 AN9 // Motor or Aux LED -#define COPTER_LED_3 AN10 // Motor LED -#define COPTER_LED_4 AN11 // Motor or GPS LED +#define COPTER_LED_1 AN8 // Motor or Aux LED +#define COPTER_LED_2 AN9 // Motor LED +#define COPTER_LED_3 AN10 // Motor or GPS LED +#define COPTER_LED_4 AN11 // Motor LED #define COPTER_LED_5 AN12 // Motor LED #define COPTER_LED_6 AN13 // Motor LED #define COPTER_LED_7 AN14 // Motor LED @@ -183,22 +182,6 @@ #endif -////////////////////////////////////////////////////////////////////////////// -// Piezo Beeper -// The Piezo Beeper output conflicts with APM2 CopterLED output on AN5 -// - -#ifndef PIEZO -# define PIEZO ENABLED //Enables Piezo Code and beeps once on Startup to verify operation -#endif -#ifndef PIEZO_LOW_VOLTAGE -# define PIEZO_LOW_VOLTAGE ENABLED //Enables Tone on reaching low battery or current alert -#endif -#ifndef PIEZO_ARMING -# define PIEZO_ARMING ENABLED //Two tones on ARM, 1 Tone on disarm -#endif - - ////////////////////////////////////////////////////////////////////////////// // Pushbutton & Relay // diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index 43d76a97d6..bb090ef977 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -94,24 +94,3 @@ static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_R } } -#if PIEZO == ENABLED -void piezo_on() -{ - digitalWrite(PIEZO_PIN,HIGH); - //PORTF |= B00100000; -} - -void piezo_off() -{ - digitalWrite(PIEZO_PIN,LOW); - //PORTF &= ~B00100000; -} - -void piezo_beep() -{ - // Note: This command should not be used in time sensitive loops - piezo_on(); - delay(100); - piezo_off(); -} -#endif diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index 95557b5603..e879dc707e 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -99,34 +99,42 @@ static void clear_leds() ///////////////////////////////////////////////////////////////////////////////////////////// // Copter LEDS by Robert Lefebvre -// Based on the work of U4eake, Bill Sanford, and Max Levine +// 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 copter leds on and off: 00000001 +// 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 // 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 +// Piezo Code and beeps once on Startup to verify operation +// Piezo Enables Tone on reaching low battery or current alert ///////////////////////////////////////////////////////////////////////////////////////////// #if COPTER_LEDS == ENABLED static void update_copter_leds(void) { - if (g.copter_leds_mode == 0) { //method of reintializing LED state - copter_leds_reset(); + 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 (low_batt == true) { - copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds oscillate + if ( bitRead(g.copter_leds_mode, 4 )) { + 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 { - copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON + copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON } } else { - copter_leds_blink(); //if motors are not armed, blink motor leds + copter_leds_slow_blink(); //if motors are not armed, blink motor leds } } @@ -138,29 +146,29 @@ 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 + 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 + 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 + 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 + 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 + 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 + copter_leds_aux_off(); //if sub-control of Ch7 is low, turn Aux LED off } } @@ -168,9 +176,7 @@ static void update_copter_leds(void) static void copter_leds_reset(void) { digitalWrite(COPTER_LED_1, COPTER_LED_OFF); - if (CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 || PIEZO == DISABLED){ - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); // The Piezo Beeper output conflicts with APM2 CopterLED output on AN5 - } + digitalWrite(COPTER_LED_2, COPTER_LED_OFF); digitalWrite(COPTER_LED_3, COPTER_LED_OFF); digitalWrite(COPTER_LED_4, COPTER_LED_OFF); digitalWrite(COPTER_LED_5, COPTER_LED_OFF); @@ -180,40 +186,48 @@ static void copter_leds_reset(void) { } static void copter_leds_on(void) { - digitalWrite(COPTER_LED_1, COPTER_LED_ON); - digitalWrite(COPTER_LED_3, COPTER_LED_ON); + if ( !bitRead(g.copter_leds_mode, 2) ){ + digitalWrite(COPTER_LED_1, COPTER_LED_ON); + } + #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + if ( !bitRead(g.copter_leds_mode, 3) ){ + digitalWrite(COPTER_LED_2, COPTER_LED_ON); + } + #else + digitalWrite(COPTER_LED_2, COPTER_LED_ON); + #endif + if ( !bitRead(g.copter_leds_mode, 1) ){ + digitalWrite(COPTER_LED_3, COPTER_LED_ON); + } + digitalWrite(COPTER_LED_4, COPTER_LED_ON); digitalWrite(COPTER_LED_5, COPTER_LED_ON); digitalWrite(COPTER_LED_6, COPTER_LED_ON); digitalWrite(COPTER_LED_7, COPTER_LED_ON); digitalWrite(COPTER_LED_8, COPTER_LED_ON); - if (CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 || PIEZO == DISABLED){ // The Piezo Beeper output conflicts with APM2 CopterLED output on AN5 - if ( !bitRead(g.copter_leds_mode, 2) ){ - digitalWrite(COPTER_LED_2, COPTER_LED_ON); - } - } - if ( !bitRead(g.copter_leds_mode, 1) ){ - digitalWrite(COPTER_LED_4, COPTER_LED_ON); - } } static void copter_leds_off(void) { - digitalWrite(COPTER_LED_1, COPTER_LED_OFF); - digitalWrite(COPTER_LED_3, COPTER_LED_OFF); + if ( !bitRead(g.copter_leds_mode, 2) ){ + digitalWrite(COPTER_LED_1, COPTER_LED_OFF); + } + #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + if ( !bitRead(g.copter_leds_mode, 3) ){ + digitalWrite(COPTER_LED_2, COPTER_LED_OFF); + } + #else + digitalWrite(COPTER_LED_2, COPTER_LED_OFF); + #endif + if ( !bitRead(g.copter_leds_mode, 1) ){ + digitalWrite(COPTER_LED_3, COPTER_LED_OFF); + } + digitalWrite(COPTER_LED_4, COPTER_LED_OFF); digitalWrite(COPTER_LED_5, COPTER_LED_OFF); digitalWrite(COPTER_LED_6, COPTER_LED_OFF); digitalWrite(COPTER_LED_7, COPTER_LED_OFF); digitalWrite(COPTER_LED_8, COPTER_LED_OFF); - if (CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 || PIEZO == DISABLED){ // The Piezo Beeper output conflicts with APM2 CopterLED output on AN5 - if ( !bitRead(g.copter_leds_mode, 2) ){ - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); - } - } - if ( !bitRead(g.copter_leds_mode, 1) ){ - digitalWrite(COPTER_LED_4, COPTER_LED_OFF); - } } -static void copter_leds_blink(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(); @@ -223,38 +237,57 @@ static void copter_leds_blink(void) { 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_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 +} + + 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, COPTER_LED_ON); - digitalWrite(COPTER_LED_3, COPTER_LED_OFF); + if ( !bitRead(g.copter_leds_mode, 2) ){ + digitalWrite(COPTER_LED_1, COPTER_LED_ON); + } + #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + if ( !bitRead(g.copter_leds_mode, 3) ){ + digitalWrite(COPTER_LED_2, COPTER_LED_ON); + } + #else + digitalWrite(COPTER_LED_2, COPTER_LED_ON); + #endif + if ( !bitRead(g.copter_leds_mode, 1) ){ + digitalWrite(COPTER_LED_3, COPTER_LED_OFF); + } + digitalWrite(COPTER_LED_4, COPTER_LED_OFF); digitalWrite(COPTER_LED_5, COPTER_LED_ON); digitalWrite(COPTER_LED_6, COPTER_LED_ON); digitalWrite(COPTER_LED_7, COPTER_LED_OFF); digitalWrite(COPTER_LED_8, COPTER_LED_OFF); - if (CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 || PIEZO == DISABLED){ // The Piezo Beeper output conflicts with APM2 CopterLED output on AN5 - if ( !bitRead(g.copter_leds_mode, 2) ) { - digitalWrite(COPTER_LED_2, COPTER_LED_ON); - } - } - if ( !bitRead(g.copter_leds_mode, 1) ) { - digitalWrite(COPTER_LED_4, COPTER_LED_OFF); - } }else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) { - digitalWrite(COPTER_LED_1, COPTER_LED_OFF); - digitalWrite(COPTER_LED_3, COPTER_LED_ON); + if ( !bitRead(g.copter_leds_mode, 2) ){ + digitalWrite(COPTER_LED_1, COPTER_LED_OFF); + } + #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + if ( !bitRead(g.copter_leds_mode, 3) ){ + digitalWrite(COPTER_LED_2, COPTER_LED_OFF); + } + #else + digitalWrite(COPTER_LED_2, COPTER_LED_OFF); + #endif + if ( !bitRead(g.copter_leds_mode, 1) ){ + digitalWrite(COPTER_LED_3, COPTER_LED_ON); + } + digitalWrite(COPTER_LED_4, COPTER_LED_ON); digitalWrite(COPTER_LED_5, COPTER_LED_OFF); digitalWrite(COPTER_LED_6, COPTER_LED_OFF); digitalWrite(COPTER_LED_7, COPTER_LED_ON); digitalWrite(COPTER_LED_8, COPTER_LED_ON); - if (CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 || PIEZO == DISABLED){ // The Piezo Beeper output conflicts with APM2 CopterLED output on AN5 - if ( !bitRead(g.copter_leds_mode, 2) ) { - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); - } - } - if ( !bitRead(g.copter_leds_mode, 1) ) { - digitalWrite(COPTER_LED_4, COPTER_LED_ON); - } } else copter_leds_motor_blink = 0; // start blink cycle again } @@ -262,11 +295,11 @@ static void copter_leds_oscillate(void) { static void copter_leds_GPS_on(void) { - digitalWrite(COPTER_LED_4, COPTER_LED_ON); + digitalWrite(COPTER_LED_3, COPTER_LED_ON); } static void copter_leds_GPS_off(void) { - digitalWrite(COPTER_LED_4, COPTER_LED_OFF); + digitalWrite(COPTER_LED_3, COPTER_LED_OFF); } static void copter_leds_GPS_slow_blink(void) { @@ -290,15 +323,25 @@ static void copter_leds_GPS_fast_blink(void) { } static void copter_leds_aux_off(void){ - if (CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 || PIEZO == DISABLED){ // The Piezo Beeper output conflicts with APM2 CopterLED output on AN5 - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); - } + digitalWrite(COPTER_LED_1, COPTER_LED_OFF); } static void copter_leds_aux_on(void){ - if (CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 || PIEZO == DISABLED){ // The Piezo Beeper output conflicts with APM2 CopterLED output on AN5 - digitalWrite(COPTER_LED_2, COPTER_LED_ON); - } + digitalWrite(COPTER_LED_1, COPTER_LED_ON); +} + +void piezo_on(){ + digitalWrite(PIEZO_PIN,HIGH); +} + +void piezo_off(){ + digitalWrite(PIEZO_PIN,LOW); +} + +void piezo_beep(){ // Note! This command should not be used in time sensitive loops + piezo_on(); + delay(100); + piezo_off(); } #endif //COPTER_LEDS diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 479e294da0..ee061fa9d9 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -84,10 +84,11 @@ static void init_arm_motors() } motors.armed(true); - #if PIEZO_ARMING == 1 - piezo_beep(); - piezo_beep(); - #endif + if ( bitRead(g.copter_leds_mode, 3) ){ + piezo_beep(); + delay(50); + piezo_beep(); + } // Remember Orientation // -------------------- @@ -137,9 +138,9 @@ static void init_disarm_motors() // we are not in the air takeoff_complete = false; - #if PIEZO_ARMING == 1 - piezo_beep(); - #endif + if ( bitRead(g.copter_leds_mode, 3) ){ + piezo_beep(); + } } /***************************************** diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 7cf21bcd88..07e5066867 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -135,21 +135,18 @@ static void read_battery(void) if((battery_voltage1 < g.low_voltage) || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity)){ low_battery_event(); - #if PIEZO_LOW_VOLTAGE == 1 - // Only Activate if a battery is connected to avoid alarm on USB only - if (battery_voltage1 > 1){ - piezo_on(); - }else{ - piezo_off(); + 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(); + }else{ + piezo_off(); + } } - #endif - }else{ - #if PIEZO_LOW_VOLTAGE == 1 - piezo_off(); - #endif + }elseif ( bitRead(g.copter_leds_mode, 3) ){ + piezo_off(); } - #endif + #endif //BATTERY_EVENT } //v: 10.9453, a: 17.4023, mah: 8.2 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 121d9ece11..3d0aee089c 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -140,12 +140,13 @@ static void init_ardupilot() 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 -#endif - - #if PIEZO == 1 - pinMode(PIEZO_PIN,OUTPUT); + + if ( !bitRead(g.copter_leds_mode, 3) ){ piezo_beep(); - #endif + } + +#endif + // load parameters from EEPROM load_parameters();