aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter - LEDS - main code to support COPTER_LEDS.

Browse Source 
Code by Robert Lefebvre
This commit is contained in:
rmackay9 2012-04-12 22:54:14 +09:00
parent c337df11c8
commit 24b81db20e
2 changed files with 199 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/ArduCopter.pde
View File

@ -454,6 +454,10 @@ static boolean GPS_light;
// This is current status for the LED lights state machine // This is current status for the LED lights state machine
// setting this value changes the output of the LEDs // setting this value changes the output of the LEDs
static byte led_mode = NORMAL_LEDS; static byte led_mode = NORMAL_LEDS;
// Blinking indicates GPS status
static byte copter_leds_GPS_blink = 0;
// Blinking indicates battery status
static byte copter_leds_motor_blink = 0;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// GPS variables // GPS variables
@ -1121,6 +1125,10 @@ static void medium_loop()
USERHOOK_MEDIUMLOOP USERHOOK_MEDIUMLOOP
#endif #endif
#if COPTER_LEDS == ENABLED
update_copter_leds();
#endif
slow_loop(); slow_loop();
break; break;
@ -1239,10 +1247,6 @@ static void slow_loop()
if(g.radio_tuning > 0) if(g.radio_tuning > 0)
tuning(); tuning();
#if MOTOR_LEDS == 1
update_motor_leds();
#endif
#if USB_MUX_PIN > 0 #if USB_MUX_PIN > 0
check_usb_mux(); check_usb_mux();
#endif #endif

220
ArduCopter/leds.pde
View File

@ -97,39 +97,201 @@ static void clear_leds()
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
} }
#if MOTOR_LEDS == 1 /////////////////////////////////////////////////////////////////////////////////////////////
static void update_motor_leds(void) // Copter LEDS by Robert Lefebvre
{ // Based on the work of U4eake, Bill Sanford, and Max Levine
if (motors.armed()){ // g.copter_leds_mode controls the copter leds function via bitmath
if (low_batt == true){ // Zeroeth bit turns copter leds on and off: 00000001
// blink rear // First bit turns GPS function on and off: 00000010
static bool blink = false; // Second bit turns Aux function on and off: 00000100
// 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
/////////////////////////////////////////////////////////////////////////////////////////////
if (blink){
digitalWrite(RE_LED, LED_ON); #if COPTER_LEDS == ENABLED
digitalWrite(FR_LED, LED_ON); static void update_copter_leds(void)
digitalWrite(RI_LED, LED_OFF); {
digitalWrite(LE_LED, LED_OFF); if (g.copter_leds_mode == 0) { //method of reintializing LED state
}else{ copter_leds_reset();
digitalWrite(RE_LED, LED_OFF); }
digitalWrite(FR_LED, LED_OFF);
digitalWrite(RI_LED, LED_ON); if ( bitRead(g.copter_leds_mode, 0) ) {
digitalWrite(LE_LED, LED_ON); if (motors.armed() == true) {
if (low_batt == true) {
copter_leds_oscillate(); //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
} }
blink = !blink; } else {
}else{ copter_leds_blink(); //if motors are not armed, blink motor leds
digitalWrite(RE_LED, LED_ON);
digitalWrite(FR_LED, LED_ON);
digitalWrite(RI_LED, LED_ON);
digitalWrite(LE_LED, LED_ON);
} }
}else { }
digitalWrite(RE_LED, LED_OFF);
digitalWrite(FR_LED, LED_OFF); if ( bitRead(g.copter_leds_mode, 1) ){
digitalWrite(RI_LED, LED_OFF);
digitalWrite(LE_LED, LED_OFF); // GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// ---------------------------------------------------------------------
switch (g_gps->status()){
case(2):
if(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
}
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) {
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
}
}
}
static void copter_leds_reset(void) {
digitalWrite(COPTER_LED_1, LED_OFF);
digitalWrite(COPTER_LED_2, LED_OFF);
digitalWrite(COPTER_LED_3, LED_OFF);
digitalWrite(COPTER_LED_4, LED_OFF);
digitalWrite(COPTER_LED_5, LED_OFF);
digitalWrite(COPTER_LED_6, LED_OFF);
digitalWrite(COPTER_LED_7, LED_OFF);
digitalWrite(COPTER_LED_8, LED_OFF);
}
static void copter_leds_on(void) {
digitalWrite(COPTER_LED_1, LED_ON);
digitalWrite(COPTER_LED_2, LED_ON);
digitalWrite(COPTER_LED_3, LED_ON);
digitalWrite(COPTER_LED_4, LED_ON);
if ( !bitRead(g.copter_leds_mode, 2) ){
digitalWrite(COPTER_LED_5, LED_ON);
digitalWrite(COPTER_LED_6, LED_ON);
}
if ( !bitRead(g.copter_leds_mode, 1) ){
digitalWrite(COPTER_LED_7, LED_ON);
digitalWrite(COPTER_LED_8, LED_ON);
} }
} }
#endif
static void copter_leds_off(void) {
digitalWrite(COPTER_LED_1, LED_OFF);
digitalWrite(COPTER_LED_2, LED_OFF);
digitalWrite(COPTER_LED_3, LED_OFF);
digitalWrite(COPTER_LED_4, LED_OFF);
if ( !bitRead(g.copter_leds_mode, 2) ){
digitalWrite(COPTER_LED_5, LED_OFF);
digitalWrite(COPTER_LED_6, LED_OFF);
}
if ( !bitRead(g.copter_leds_mode, 1) ){
digitalWrite(COPTER_LED_7, LED_OFF);
digitalWrite(COPTER_LED_8, LED_OFF);
}
}
static void copter_leds_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();
}else if (5 < copter_leds_motor_blink && copter_leds_motor_blink < 11){
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, LED_ON);
digitalWrite(COPTER_LED_2, LED_ON);
digitalWrite(COPTER_LED_3, LED_OFF);
digitalWrite(COPTER_LED_4, LED_OFF);
if ( !bitRead(g.copter_leds_mode, 2) ) {
digitalWrite(COPTER_LED_5, LED_ON);
digitalWrite(COPTER_LED_6, LED_ON);
}
if ( !bitRead(g.copter_leds_mode, 1) ) {
digitalWrite(COPTER_LED_7, LED_OFF);
digitalWrite(COPTER_LED_8, LED_OFF);
}
}else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) {
digitalWrite(COPTER_LED_1, LED_OFF);
digitalWrite(COPTER_LED_2, LED_OFF);
digitalWrite(COPTER_LED_3, LED_ON);
digitalWrite(COPTER_LED_4, LED_ON);
if ( !bitRead(g.copter_leds_mode, 2) ) {
digitalWrite(COPTER_LED_5, LED_OFF);
digitalWrite(COPTER_LED_6, LED_OFF);
}
if ( !bitRead(g.copter_leds_mode, 1) ) {
digitalWrite(COPTER_LED_7, LED_ON);
digitalWrite(COPTER_LED_8, LED_ON);
}
}
else copter_leds_motor_blink = 0; // start blink cycle again
}
static void copter_leds_GPS_on(void) {
digitalWrite(COPTER_LED_7, LED_ON);
digitalWrite(COPTER_LED_8, LED_ON);
}
static void copter_leds_GPS_off(void) {
digitalWrite(COPTER_LED_7, LED_OFF);
digitalWrite(COPTER_LED_8, 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();
}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_5, LED_OFF);
digitalWrite(COPTER_LED_6, LED_OFF);
}
static void copter_leds_aux_on(void)
{
digitalWrite(COPTER_LED_5, LED_ON);
digitalWrite(COPTER_LED_6, LED_ON);
}
#endif //COPTER_LEDS