diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index de9e2b1899..d924314d6a 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -28,7 +28,6 @@ //#define OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space //#define AC_FENCE DISABLED // disable fence to save 2k of flash //#define CAMERA DISABLED // disable camera trigger to save 1k of flash -//#define COPTER_LEDS DISABLED // disable external navigation leds to save 1k of flash //#define CONFIG_SONAR DISABLED // disable sonar to save 1k of flash // features below are disabled by default diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index fa185dd356..f1baf38af4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -485,15 +485,6 @@ float tuning_value; // used to limit the rate that the pid controller output is logged so that it doesn't negatively affect performance static uint8_t pid_log_counter; -//////////////////////////////////////////////////////////////////////////////// -// LED output -//////////////////////////////////////////////////////////////////////////////// -// Blinking indicates GPS status -static uint8_t copter_leds_GPS_blink; -// Blinking indicates battery status -static uint8_t copter_leds_motor_blink; -// Navigation confirmation blinks -static int8_t copter_leds_nav_blink; //////////////////////////////////////////////////////////////////////////////// // GPS variables diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 3a257857de..14f157c792 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -162,7 +162,7 @@ public: k_param_sonar_type, k_param_super_simple = 155, k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change - k_param_copter_leds_mode, + k_param_copter_leds_mode, // deprecated - remove with next eeprom number change k_param_ahrs, // AHRS group // 159 // @@ -308,8 +308,6 @@ public: AP_Int8 optflow_enabled; AP_Int8 super_simple; AP_Int16 rtl_alt_final; - AP_Int8 copter_leds_mode; // Operating mode of LED - // lighting system AP_Int8 rssi_pin; AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index e8f851cffc..457fc9df35 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -591,13 +591,6 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(acro_trainer, "ACRO_TRAINER", ACRO_TRAINER_LIMITED), - // @Param: LED_MODE - // @DisplayName: Copter LED Mode - // @Description: bitmap to control the copter led mode - // @Values: 0:Disabled,1:Enable,3:GPS On,4:Aux,9:Buzzer,17:Oscillate,33:Nav Blink,65:GPS Nav Blink - // @User: Standard - GSCALAR(copter_leds_mode, "LED_MODE", 9), - // PID controller //--------------- // @Param: RATE_RLL_P diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 0b5ccd6c74..08563082b0 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -531,7 +531,6 @@ static bool verify_nav_wp() // check if timer has run out if (((millis() - loiter_time) / 1000) >= loiter_time_max) { gcs_send_text_fmt(PSTR("Reached Command #%i"),command_nav_index); - 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/config.h b/ArduCopter/config.h index 3392917874..515e303e38 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -193,41 +193,6 @@ # define LED_OFF HIGH #endif -//////////////////////////////////////////////////////////////////////////////// -// CopterLEDs -// - -#ifndef COPTER_LEDS - #define COPTER_LEDS ENABLED -#endif - -#define COPTER_LED_ON HIGH -#define COPTER_LED_OFF LOW - -#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 - #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_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL || CONFIG_HAL_BOARD == HAL_BOARD_PX4 - #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 - #define COPTER_LED_8 AN15 // Motor LED -#else - // not supported yet on this board - #undef COPTER_LEDS -#endif - - ////////////////////////////////////////////////////////////////////////////// // Barometer // diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index ed55301654..3e4f1bfe9f 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -242,9 +242,6 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) // log event Log_Write_Event(DATA_SAVEWP_ADD_WP); - - // Cause the CopterLEDs to blink twice to indicate saved waypoint - copter_leds_nav_blink = 10; } break; diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index d9b7a7d758..13c69776ef 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -164,9 +164,6 @@ static void failsafe_battery_event(void) gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!")); Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); -#if COPTER_LEDS == ENABLED - piezo_on(); -#endif // COPTER_LEDS } // failsafe_gps_check - check for gps failsafe diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index 681b32e399..a1dac561fd 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -7,316 +7,3 @@ static void update_notify() notify.update(); } -///////////////////////////////////////////////////////////////////////////////////////////// -// 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: 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 -// 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 -///////////////////////////////////////////////////////////////////////////////////////////// - -#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 - } - - // motor leds control - if (g.copter_leds_mode & COPTER_LEDS_BITMASK_ENABLED) { - if (motors.armed()) { - if (failsafe.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 (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(); //if motors are armed, battery level OK, all motor leds ON - } - } - } else { - copter_leds_slow_blink(); //if motors are not armed, blink motor leds - } - } - - // 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 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 (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; - } - } - - // AUX led control - if (g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX) { - if (ap.CH7_flag) { - copter_leds_aux_on(); //if sub-control of Ch7 is high, turn Aux LED on - } else { - 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, COPTER_LED_OFF); - 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); - digitalWrite(COPTER_LED_6, COPTER_LED_OFF); - digitalWrite(COPTER_LED_7, COPTER_LED_OFF); - digitalWrite(COPTER_LED_8, COPTER_LED_OFF); -} - -static void copter_leds_on(void) { - if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) { - digitalWrite(COPTER_LED_1, COPTER_LED_ON); - } - #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 - if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) { - digitalWrite(COPTER_LED_2, COPTER_LED_ON); - } - #else - digitalWrite(COPTER_LED_2, COPTER_LED_ON); - #endif - if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS) ) { - 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); -} - -static void copter_leds_off(void) { - if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) { - digitalWrite(COPTER_LED_1, COPTER_LED_OFF); - } - #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 - if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) { - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); - } - #else - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); - #endif - if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS)) { - 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); -} - -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_off(); - - // 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 - } -} - -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 - if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) { - digitalWrite(COPTER_LED_1, COPTER_LED_ON); - } - #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 - if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) { - digitalWrite(COPTER_LED_2, COPTER_LED_ON); - } - #else - digitalWrite(COPTER_LED_2, COPTER_LED_ON); - #endif - if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS)) { - 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); - }else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) { - if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) { - digitalWrite(COPTER_LED_1, COPTER_LED_OFF); - } - #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 - if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) { - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); - } - #else - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); - #endif - if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS) ) { - 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); - }else{ - copter_leds_motor_blink = 0; // start blink cycle again - } -} - -static void copter_leds_GPS_on(void) { - digitalWrite(COPTER_LED_3, COPTER_LED_ON); -} - -static void copter_leds_GPS_off(void) { - digitalWrite(COPTER_LED_3, COPTER_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(); - 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) { - 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_1, COPTER_LED_OFF); -} - -static void copter_leds_aux_on(void){ - digitalWrite(COPTER_LED_1, COPTER_LED_ON); -} - -void piezo_on(){ - if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) { - digitalWrite(PIEZO_PIN,HIGH); - } -} - -void piezo_off(){ - if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) { - digitalWrite(PIEZO_PIN,LOW); - } -} - -void piezo_beep(){ // Note! This command should not be used in time sensitive loops - 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 c84a749a2a..bc14960134 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -146,10 +146,6 @@ static void init_arm_motors() hal.uartD->set_blocking_writes(false); } -#if COPTER_LEDS == ENABLED - piezo_beep_twice(); -#endif - // Remember Orientation // -------------------- init_simple_bearing(); @@ -188,10 +184,6 @@ static void init_arm_motors() // set hover throttle motors.set_mid_throttle(g.throttle_mid); -#if COPTER_LEDS == ENABLED - piezo_beep_twice(); -#endif - // Cancel arming if throttle is raised too high so that copter does not suddenly take off read_radio(); if (g.rc_3.control_in > g.throttle_cruise && g.throttle_cruise > 100) { @@ -496,11 +488,7 @@ static void init_disarm_motors() // we are not in the air set_takeoff_complete(false); - -#if COPTER_LEDS == ENABLED - piezo_beep(); -#endif - + // setup fast AHRS gains to get right attitude ahrs.set_fast_gains(true); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index e572444987..0b5bd88345 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -122,10 +122,6 @@ static void init_ardupilot() relay.init(); -#if COPTER_LEDS == ENABLED - copter_leds_init(); -#endif - // load parameters from EEPROM load_parameters();