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();