mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Copter LEDs: replace bitRead with bitmask
Consolidate all checks of led_mode to leds.pde Add #defines for bitmasks comparisons Some formatting changes
This commit is contained in:
Randy Mackay
parent
6816c45c39
commit
7cf2255822
@ -11,8 +11,5 @@
|
|||||||
/* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */
|
/* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */
|
||||||
static void run_cli(AP_HAL::UARTDriver *port);
|
static void run_cli(AP_HAL::UARTDriver *port);
|
||||||
|
|
||||||
// XXX AP_HAL tofix - what is bitRead? temporarily disable.
|
|
||||||
#define bitRead(a,b) false
|
|
||||||
|
|
||||||
#endif // __COMPAT_H__
|
#endif // __COMPAT_H__
|
||||||
|
|
||||||
|
|||||||
@ -98,9 +98,8 @@ static void low_battery_event(void)
|
|||||||
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
|
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
|
||||||
|
|
||||||
#if COPTER_LEDS == ENABLED
|
#if COPTER_LEDS == ENABLED
|
||||||
if ( bitRead(g.copter_leds_mode, 3) ) { // Only Activate if a battery is connected to avoid alarm on USB only
|
// Only Activate if a battery is connected to avoid alarm on USB only
|
||||||
piezo_on();
|
piezo_on();
|
||||||
}
|
|
||||||
#endif // COPTER_LEDS
|
#endif // COPTER_LEDS
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -119,29 +119,55 @@ static void clear_leds()
|
|||||||
// Piezo Enables Tone on reaching low battery or current alert
|
// 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
|
#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)
|
static void update_copter_leds(void)
|
||||||
{
|
{
|
||||||
if (g.copter_leds_mode == 0) {
|
if (g.copter_leds_mode == 0) {
|
||||||
copter_leds_reset(); //method of reintializing LED state
|
copter_leds_reset(); //method of reintializing LED state
|
||||||
}
|
}
|
||||||
|
|
||||||
if ( bitRead(g.copter_leds_mode, 0) ) {
|
// motor leds control
|
||||||
if (motors.armed() == true) {
|
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_ENABLED) {
|
||||||
if (ap.low_battery == true) {
|
if (motors.armed()) {
|
||||||
if ( bitRead(g.copter_leds_mode, 4 )) {
|
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
|
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
|
copter_leds_fast_blink(); //if motors are armed, but battery level is low, motor leds oscillate
|
||||||
}
|
}
|
||||||
} else if ( !bitRead(g.copter_leds_mode, 5 ) ) {
|
} else {
|
||||||
copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON
|
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_MOTOR_NAV_BLINK) {
|
||||||
} else if ( bitRead(g.copter_leds_mode, 5 ) ) {
|
if (copter_leds_nav_blink > 0) {
|
||||||
if ( copter_leds_nav_blink >0 ) {
|
copter_leds_slow_blink(); //if nav command was seen, blink LEDs.
|
||||||
copter_leds_slow_blink(); //if nav command was seen, blink LEDs.
|
} else {
|
||||||
|
copter_leds_on();
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
copter_leds_on();
|
copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
@ -149,48 +175,48 @@ static void update_copter_leds(void)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if ( bitRead(g.copter_leds_mode, 1) ) {
|
// 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
|
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
|
||||||
// ---------------------------------------------------------------------
|
// ---------------------------------------------------------------------
|
||||||
switch (g_gps->status()) {
|
switch (g_gps->status()) {
|
||||||
|
|
||||||
case (2):
|
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(ap.home_is_set) {
|
||||||
if ( !bitRead(g.copter_leds_mode, 6 ) ) {
|
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS_NAV_BLINK) {
|
||||||
copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set
|
if (copter_leds_nav_blink >0) {
|
||||||
} 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.
|
copter_leds_GPS_slow_blink(); //if nav command was seen, blink LEDs.
|
||||||
} else {
|
} else {
|
||||||
copter_leds_GPS_on();
|
copter_leds_GPS_on();
|
||||||
}
|
}
|
||||||
|
} else {
|
||||||
|
copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set
|
||||||
}
|
}
|
||||||
} else {
|
} 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;
|
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) ) {
|
// AUX led control
|
||||||
if (200 <= g.rc_7.control_in && g.rc_7.control_in < 400) {
|
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
|
copter_leds_aux_on(); //if sub-control of Ch7 is high, turn Aux LED on
|
||||||
} else if (g.rc_7.control_in < 200) {
|
} else {
|
||||||
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
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static void copter_leds_reset(void) {
|
static void copter_leds_reset(void) {
|
||||||
@ -205,17 +231,17 @@ static void copter_leds_reset(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
static void copter_leds_on(void) {
|
static void copter_leds_on(void) {
|
||||||
if ( !bitRead(g.copter_leds_mode, 2) ) {
|
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) {
|
||||||
digitalWriteFast(COPTER_LED_1, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_1, COPTER_LED_ON);
|
||||||
}
|
}
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||||
if ( !bitRead(g.copter_leds_mode, 3) ) {
|
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) {
|
||||||
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
|
||||||
#endif
|
#endif
|
||||||
if ( !bitRead(g.copter_leds_mode, 1) ) {
|
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS) ) {
|
||||||
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
|
||||||
}
|
}
|
||||||
digitalWriteFast(COPTER_LED_4, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_4, COPTER_LED_ON);
|
||||||
@ -226,17 +252,17 @@ static void copter_leds_on(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
static void copter_leds_off(void) {
|
static void copter_leds_off(void) {
|
||||||
if ( !bitRead(g.copter_leds_mode, 2) ) {
|
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) {
|
||||||
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
|
||||||
}
|
}
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||||
if ( !bitRead(g.copter_leds_mode, 3) ) {
|
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) {
|
||||||
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
|
||||||
#endif
|
#endif
|
||||||
if ( !bitRead(g.copter_leds_mode, 1) ) {
|
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS)) {
|
||||||
digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF);
|
||||||
}
|
}
|
||||||
digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF);
|
||||||
@ -247,43 +273,47 @@ static void copter_leds_off(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
static void copter_leds_slow_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
|
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
|
|
||||||
|
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();
|
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
|
// 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) {
|
}else if (5 < copter_leds_motor_blink && copter_leds_motor_blink < 11) {
|
||||||
copter_leds_on();
|
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) {
|
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
|
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 ( 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();
|
copter_leds_on();
|
||||||
}else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) {
|
}else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) {
|
||||||
copter_leds_off();
|
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) {
|
static void copter_leds_oscillate(void) {
|
||||||
copter_leds_motor_blink++; // this increments once every 1/10 second because it is in the 10hz loop
|
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 ( 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) ) {
|
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) {
|
||||||
digitalWriteFast(COPTER_LED_1, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_1, COPTER_LED_ON);
|
||||||
}
|
}
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||||
if ( !bitRead(g.copter_leds_mode, 3) ) {
|
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) {
|
||||||
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
|
||||||
#endif
|
#endif
|
||||||
if ( !bitRead(g.copter_leds_mode, 1) ) {
|
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS)) {
|
||||||
digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF);
|
||||||
}
|
}
|
||||||
digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF);
|
||||||
@ -292,17 +322,17 @@ static void copter_leds_oscillate(void) {
|
|||||||
digitalWriteFast(COPTER_LED_7, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_7, COPTER_LED_OFF);
|
||||||
digitalWriteFast(COPTER_LED_8, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_8, COPTER_LED_OFF);
|
||||||
}else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) {
|
}else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) {
|
||||||
if ( !bitRead(g.copter_leds_mode, 2) ) {
|
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) {
|
||||||
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
|
||||||
}
|
}
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||||
if ( !bitRead(g.copter_leds_mode, 3) ) {
|
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) {
|
||||||
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
|
||||||
#endif
|
#endif
|
||||||
if ( !bitRead(g.copter_leds_mode, 1) ) {
|
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS) ) {
|
||||||
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
|
||||||
}
|
}
|
||||||
digitalWriteFast(COPTER_LED_4, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_4, COPTER_LED_ON);
|
||||||
@ -310,12 +340,11 @@ static void copter_leds_oscillate(void) {
|
|||||||
digitalWriteFast(COPTER_LED_6, COPTER_LED_OFF);
|
digitalWriteFast(COPTER_LED_6, COPTER_LED_OFF);
|
||||||
digitalWriteFast(COPTER_LED_7, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_7, COPTER_LED_ON);
|
||||||
digitalWriteFast(COPTER_LED_8, COPTER_LED_ON);
|
digitalWriteFast(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
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
static void copter_leds_GPS_on(void) {
|
static void copter_leds_GPS_on(void) {
|
||||||
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
|
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
|
||||||
}
|
}
|
||||||
@ -328,7 +357,7 @@ 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
|
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
|
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();
|
copter_leds_GPS_off();
|
||||||
if ( bitRead(g.copter_leds_mode, 6 ) && copter_leds_nav_blink >0 ) { // if blinking is called by the Nav Blinker...
|
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
|
copter_leds_nav_blink--; // decrement the Nav Blink counter
|
||||||
}
|
}
|
||||||
}else if (5 < copter_leds_GPS_blink && copter_leds_GPS_blink < 11) {
|
}else if (5 < copter_leds_GPS_blink && copter_leds_GPS_blink < 11) {
|
||||||
@ -356,17 +385,31 @@ static void copter_leds_aux_on(void){
|
|||||||
}
|
}
|
||||||
|
|
||||||
void piezo_on(){
|
void piezo_on(){
|
||||||
digitalWriteFast(PIEZO_PIN,HIGH);
|
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) {
|
||||||
|
digitalWriteFast(PIEZO_PIN,HIGH);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void piezo_off(){
|
void piezo_off(){
|
||||||
digitalWriteFast(PIEZO_PIN,LOW);
|
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) {
|
||||||
|
digitalWriteFast(PIEZO_PIN,LOW);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void piezo_beep(){ // Note! This command should not be used in time sensitive loops
|
void piezo_beep(){ // Note! This command should not be used in time sensitive loops
|
||||||
piezo_on();
|
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) {
|
||||||
delay(100);
|
piezo_on();
|
||||||
piezo_off();
|
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
|
#endif //COPTER_LEDS
|
||||||
|
|||||||
@ -120,11 +120,7 @@ static void init_arm_motors()
|
|||||||
}
|
}
|
||||||
|
|
||||||
#if COPTER_LEDS == ENABLED
|
#if COPTER_LEDS == ENABLED
|
||||||
if ( bitRead(g.copter_leds_mode, 3) ) {
|
piezo_beep_twice();
|
||||||
piezo_beep();
|
|
||||||
delay(50);
|
|
||||||
piezo_beep();
|
|
||||||
}
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Remember Orientation
|
// Remember Orientation
|
||||||
@ -191,9 +187,7 @@ static void init_disarm_motors()
|
|||||||
set_takeoff_complete(false);
|
set_takeoff_complete(false);
|
||||||
|
|
||||||
#if COPTER_LEDS == ENABLED
|
#if COPTER_LEDS == ENABLED
|
||||||
if ( bitRead(g.copter_leds_mode, 3) ) {
|
piezo_beep();
|
||||||
piezo_beep();
|
|
||||||
}
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// setup fast AHRS gains to get right attitude
|
// setup fast AHRS gains to get right attitude
|
||||||
|
|||||||
@ -136,22 +136,9 @@ static void init_ardupilot()
|
|||||||
relay.init();
|
relay.init();
|
||||||
|
|
||||||
#if COPTER_LEDS == ENABLED
|
#if COPTER_LEDS == ENABLED
|
||||||
pinMode(COPTER_LED_1, OUTPUT); //Motor LED
|
copter_leds_init();
|
||||||
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 ( !bitRead(g.copter_leds_mode, 3) ) {
|
|
||||||
piezo_beep();
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
// load parameters from EEPROM
|
// load parameters from EEPROM
|
||||||
load_parameters();
|
load_parameters();
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user