mirror of https://github.com/ArduPilot/ardupilot
Copter: double flash arming light when pre-arm checks fail
Hardly my finest work but it's temporary because we're going to replace all the led, buzzer etc with a library called AP_Notify in the near future.
This commit is contained in:
parent
908bde9dca
commit
db893d288b
|
@ -387,7 +387,7 @@ static union {
|
||||||
|
|
||||||
static struct AP_System{
|
static struct AP_System{
|
||||||
uint8_t GPS_light : 1; // 0 // Solid indicates we have full 3D lock and can navigate, flash = read
|
uint8_t GPS_light : 1; // 0 // Solid indicates we have full 3D lock and can navigate, flash = read
|
||||||
uint8_t motor_light : 1; // 1 // Solid indicates Armed state
|
uint8_t arming_light : 1; // 1 // Solid indicates armed state, flashing is disarmed, double flashing is disarmed and failing pre-arm checks
|
||||||
uint8_t new_radio_frame : 1; // 2 // Set true if we have new PWM data to act on from the Radio
|
uint8_t new_radio_frame : 1; // 2 // Set true if we have new PWM data to act on from the Radio
|
||||||
uint8_t CH7_flag : 1; // 3 // true if ch7 aux switch is high
|
uint8_t CH7_flag : 1; // 3 // true if ch7 aux switch is high
|
||||||
uint8_t CH8_flag : 1; // 4 // true if ch8 aux switch is high
|
uint8_t CH8_flag : 1; // 4 // true if ch8 aux switch is high
|
||||||
|
@ -1175,6 +1175,8 @@ static void medium_loop()
|
||||||
USERHOOK_MEDIUMLOOP
|
USERHOOK_MEDIUMLOOP
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// update board leds
|
||||||
|
update_board_leds();
|
||||||
#if COPTER_LEDS == ENABLED
|
#if COPTER_LEDS == ENABLED
|
||||||
update_copter_leds();
|
update_copter_leds();
|
||||||
#endif
|
#endif
|
||||||
|
@ -1254,9 +1256,6 @@ static void slow_loop()
|
||||||
slow_loopCounter = 0;
|
slow_loopCounter = 0;
|
||||||
update_events();
|
update_events();
|
||||||
|
|
||||||
// blink if we are armed
|
|
||||||
update_lights();
|
|
||||||
|
|
||||||
if(g.radio_tuning > 0)
|
if(g.radio_tuning > 0)
|
||||||
tuning();
|
tuning();
|
||||||
|
|
||||||
|
|
|
@ -1,15 +1,27 @@
|
||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
static void update_lights()
|
// update_board_leds - updates leds on board
|
||||||
|
// should be called at 10hz
|
||||||
|
static void update_board_leds()
|
||||||
{
|
{
|
||||||
|
// we need to slow down the calls to the GPS and dancing lights to 3.33hz
|
||||||
|
static uint8_t counter = 0;
|
||||||
|
if(++counter >= 3){
|
||||||
|
counter = 0;
|
||||||
|
}
|
||||||
|
|
||||||
switch(led_mode) {
|
switch(led_mode) {
|
||||||
case NORMAL_LEDS:
|
case NORMAL_LEDS:
|
||||||
update_motor_light();
|
update_arming_light();
|
||||||
|
if (counter == 0) {
|
||||||
update_GPS_light();
|
update_GPS_light();
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SAVE_TRIM_LEDS:
|
case SAVE_TRIM_LEDS:
|
||||||
|
if (counter == 0) {
|
||||||
dancing_light();
|
dancing_light();
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -47,20 +59,64 @@ static void update_GPS_light(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void update_motor_light(void)
|
static void update_arming_light(void)
|
||||||
{
|
{
|
||||||
if(motors.armed() == false) {
|
// counter to control state
|
||||||
ap_system.motor_light = !ap_system.motor_light;
|
static int8_t counter = 0;
|
||||||
|
counter++;
|
||||||
|
|
||||||
// blink
|
// disarmed
|
||||||
if(ap_system.motor_light) {
|
if(!motors.armed()) {
|
||||||
|
if(!ap.pre_arm_check) {
|
||||||
|
// failed pre-arm checks so double flash
|
||||||
|
switch(counter) {
|
||||||
|
case 0:
|
||||||
|
ap_system.arming_light = true;
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
ap_system.arming_light = false;
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
ap_system.arming_light = true;
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
case 4:
|
||||||
|
ap_system.arming_light = false;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
// reset counter to restart the sequence
|
||||||
|
counter = -1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}else{
|
||||||
|
// passed pre-arm checks so slower single flash
|
||||||
|
switch(counter) {
|
||||||
|
case 0:
|
||||||
|
case 1:
|
||||||
|
case 2:
|
||||||
|
ap_system.arming_light = true;
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
case 4:
|
||||||
|
case 5:
|
||||||
|
ap_system.arming_light = false;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
// reset counter to restart the sequence
|
||||||
|
counter = -1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// set arming led from arming_light flag
|
||||||
|
if(ap_system.arming_light) {
|
||||||
digitalWrite(A_LED_PIN, LED_ON);
|
digitalWrite(A_LED_PIN, LED_ON);
|
||||||
}else{
|
}else{
|
||||||
digitalWrite(A_LED_PIN, LED_OFF);
|
digitalWrite(A_LED_PIN, LED_OFF);
|
||||||
}
|
}
|
||||||
}else{
|
}else{
|
||||||
if(!ap_system.motor_light) {
|
// armed
|
||||||
ap_system.motor_light = true;
|
if(!ap_system.arming_light) {
|
||||||
|
ap_system.arming_light = true;
|
||||||
digitalWrite(A_LED_PIN, LED_ON);
|
digitalWrite(A_LED_PIN, LED_ON);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -91,12 +147,13 @@ static void dancing_light()
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void clear_leds()
|
static void clear_leds()
|
||||||
{
|
{
|
||||||
digitalWrite(A_LED_PIN, LED_OFF);
|
digitalWrite(A_LED_PIN, LED_OFF);
|
||||||
digitalWrite(B_LED_PIN, LED_OFF);
|
digitalWrite(B_LED_PIN, LED_OFF);
|
||||||
digitalWrite(C_LED_PIN, LED_OFF);
|
digitalWrite(C_LED_PIN, LED_OFF);
|
||||||
ap_system.motor_light = false;
|
ap_system.arming_light = false;
|
||||||
led_mode = NORMAL_LEDS;
|
led_mode = NORMAL_LEDS;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -159,10 +159,6 @@ static void init_arm_motors()
|
||||||
init_barometer();
|
init_barometer();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// temp hack
|
|
||||||
ap_system.motor_light = true;
|
|
||||||
digitalWrite(A_LED_PIN, LED_ON);
|
|
||||||
|
|
||||||
// go back to normal AHRS gains
|
// go back to normal AHRS gains
|
||||||
ahrs.set_fast_gains(false);
|
ahrs.set_fast_gains(false);
|
||||||
#if SECONDARY_DMP_ENABLED == ENABLED
|
#if SECONDARY_DMP_ENABLED == ENABLED
|
||||||
|
@ -175,6 +171,9 @@ static void init_arm_motors()
|
||||||
// set hover throttle
|
// set hover throttle
|
||||||
motors.set_mid_throttle(g.throttle_mid);
|
motors.set_mid_throttle(g.throttle_mid);
|
||||||
|
|
||||||
|
// update leds on board
|
||||||
|
update_arming_light();
|
||||||
|
|
||||||
#if COPTER_LEDS == ENABLED
|
#if COPTER_LEDS == ENABLED
|
||||||
piezo_beep_twice();
|
piezo_beep_twice();
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue