Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Updates to CopterLEDS 

Creation of Nav Blink Function.
This commit is contained in:
Robert Lefebvre 2012-05-22 09:57:38 -04:00
parent e2c36eb158
commit df4246597f
6 changed files with 61 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.pde
View File

@ -471,6 +471,8 @@ static byte led_mode = NORMAL_LEDS;
static byte copter_leds_GPS_blink = 0;
// Blinking indicates battery status
static byte copter_leds_motor_blink = 0;
// Navigation confirmation blinks
static int8_t copter_leds_nav_blink = 0;
////////////////////////////////////////////////////////////////////////////////
// GPS variables

1
ArduCopter/commands_logic.pde
View File

@ -513,6 +513,7 @@ static bool verify_nav_wp()
sprintf(message,"Reached Command #%i",command_nav_index);
gcs_send_text(SEVERITY_LOW,message);
wp_verify_byte = 0;
copter_leds_nav_blink = 15; // Cause the CopterLEDs to blink three times to indicate waypoint reached
return true;
}else{
return false;

2
ArduCopter/control_modes.pde
View File

@ -141,6 +141,8 @@ static void read_trim_switch()
// save command
set_cmd_with_index(current_loc, CH7_wp_index);
copter_leds_nav_blink = 10; // Cause the CopterLEDs to blink twice to indicate saved waypoint
// 0 = home
// 1 = takeoff
// 2 = WP 2

76
ArduCopter/leds.pde
View File

@ -101,11 +101,13 @@ static void clear_leds()
// 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: 000100000 (0) does Fast Flash, (1) does Oscillate
// 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
@ -127,11 +129,17 @@ static void update_copter_leds(void)
if (low_batt == true) {
if ( bitRead(g.copter_leds_mode, 4 )) {
copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds fast blink
}else{
} else {
copter_leds_fast_blink(); //if motors are armed, but battery level is low, motor leds oscillate
}
} else {
} else if ( !bitRead(g.copter_leds_mode, 5 ) ) {
copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON
} else if ( bitRead(g.copter_leds_mode, 5 ) ) {
if ( copter_leds_nav_blink >0 ) {
copter_leds_slow_blink(); //if nav command was seen, blink LEDs.
} else {
copter_leds_on();
}
}
} else {
copter_leds_slow_blink(); //if motors are not armed, blink motor leds
@ -146,8 +154,16 @@ 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
} else {
if ( !bitRead(g.copter_leds_mode, 6 ) ) {
copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set
} else if (bitRead(g.copter_leds_mode, 6 ) ) {
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_fast_blink(); //if GPS has fix, but home is not set, blink GPS LED fast
}
break;
@ -228,29 +244,32 @@ static void copter_leds_off(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();
}else if (5 < copter_leds_motor_blink && copter_leds_motor_blink < 11){
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 ( bitRead(g.copter_leds_mode, 5 ) && !bitRead(g.copter_leds_mode, 6 ) && 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_motor_blink && copter_leds_motor_blink < 11){
copter_leds_on();
}
else copter_leds_motor_blink = 0; // start blink cycle again
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_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
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
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 ( !bitRead(g.copter_leds_mode, 2) ){
digitalWrite(COPTER_LED_1, COPTER_LED_ON);
}
@ -289,7 +308,7 @@ static void copter_leds_oscillate(void) {
digitalWrite(COPTER_LED_7, COPTER_LED_ON);
digitalWrite(COPTER_LED_8, COPTER_LED_ON);
}
else copter_leds_motor_blink = 0; // start blink cycle again
else copter_leds_motor_blink = 0; // start blink cycle again
}
@ -303,23 +322,26 @@ static void copter_leds_GPS_off(void) {
}
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_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 ( bitRead(g.copter_leds_mode, 6 ) && 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
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_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
else copter_leds_GPS_blink = 0; // start blink cycle again
}
static void copter_leds_aux_off(void){
@ -344,4 +366,4 @@ void piezo_beep(){ // Note! This command should not be used in tim
piezo_off();
}
#endif //COPTER_LEDS
#endif //COPTER_LEDS

4
ArduCopter/motors.pde
View File

@ -84,11 +84,13 @@ static void init_arm_motors()
}
motors.armed(true);
#if COPTER_LEDS == ENABLED
if ( bitRead(g.copter_leds_mode, 3) ){
piezo_beep();
delay(50);
piezo_beep();
}
#endif
// Remember Orientation
// --------------------
@ -138,9 +140,11 @@ static void init_disarm_motors()
// we are not in the air
takeoff_complete = false;
#if COPTER_LEDS == ENABLED
if ( bitRead(g.copter_leds_mode, 3) ){
piezo_beep();
}
#endif
}
/*****************************************

3
ArduCopter/sensors.pde
View File

@ -135,6 +135,7 @@ static void read_battery(void)
if((battery_voltage1 < g.low_voltage) || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity)){
low_battery_event();
#if COPTER_LEDS == ENABLED
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();
@ -142,9 +143,11 @@ static void read_battery(void)
piezo_off();
}
}
}else if ( bitRead(g.copter_leds_mode, 3) ){
piezo_off();
#endif // COPTER_LEDS
}
#endif //BATTERY_EVENT
}