/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // updates the status of notify // should be called at 50hz 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 (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 (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_system.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