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:
Randy Mackay
parent
908bde9dca
commit
db893d288b
|
|
@ -387,7 +387,7 @@ static union {
|
|||
|
||||
static struct AP_System{
|
||||
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 CH7_flag : 1; // 3 // true if ch7 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
|
||||
#endif
|
||||
|
||||
// update board leds
|
||||
update_board_leds();
|
||||
#if COPTER_LEDS == ENABLED
|
||||
update_copter_leds();
|
||||
#endif
|
||||
|
|
@ -1254,9 +1256,6 @@ static void slow_loop()
|
|||
slow_loopCounter = 0;
|
||||
update_events();
|
||||
|
||||
// blink if we are armed
|
||||
update_lights();
|
||||
|
||||
if(g.radio_tuning > 0)
|
||||
tuning();
|
||||
|
||||
|
|
|
|||
|
|
@ -1,15 +1,27 @@
|
|||
/// -*- 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) {
|
||||
case NORMAL_LEDS:
|
||||
update_motor_light();
|
||||
update_GPS_light();
|
||||
update_arming_light();
|
||||
if (counter == 0) {
|
||||
update_GPS_light();
|
||||
}
|
||||
break;
|
||||
|
||||
case SAVE_TRIM_LEDS:
|
||||
dancing_light();
|
||||
if (counter == 0) {
|
||||
dancing_light();
|
||||
}
|
||||
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) {
|
||||
ap_system.motor_light = !ap_system.motor_light;
|
||||
// counter to control state
|
||||
static int8_t counter = 0;
|
||||
counter++;
|
||||
|
||||
// blink
|
||||
if(ap_system.motor_light) {
|
||||
// disarmed
|
||||
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);
|
||||
}else{
|
||||
digitalWrite(A_LED_PIN, LED_OFF);
|
||||
}
|
||||
}else{
|
||||
if(!ap_system.motor_light) {
|
||||
ap_system.motor_light = true;
|
||||
// armed
|
||||
if(!ap_system.arming_light) {
|
||||
ap_system.arming_light = true;
|
||||
digitalWrite(A_LED_PIN, LED_ON);
|
||||
}
|
||||
}
|
||||
|
|
@ -91,12 +147,13 @@ static void dancing_light()
|
|||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void clear_leds()
|
||||
{
|
||||
digitalWrite(A_LED_PIN, LED_OFF);
|
||||
digitalWrite(B_LED_PIN, LED_OFF);
|
||||
digitalWrite(C_LED_PIN, LED_OFF);
|
||||
ap_system.motor_light = false;
|
||||
ap_system.arming_light = false;
|
||||
led_mode = NORMAL_LEDS;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -159,10 +159,6 @@ static void init_arm_motors()
|
|||
init_barometer();
|
||||
#endif
|
||||
|
||||
// temp hack
|
||||
ap_system.motor_light = true;
|
||||
digitalWrite(A_LED_PIN, LED_ON);
|
||||
|
||||
// go back to normal AHRS gains
|
||||
ahrs.set_fast_gains(false);
|
||||
#if SECONDARY_DMP_ENABLED == ENABLED
|
||||
|
|
@ -175,6 +171,9 @@ static void init_arm_motors()
|
|||
// set hover throttle
|
||||
motors.set_mid_throttle(g.throttle_mid);
|
||||
|
||||
// update leds on board
|
||||
update_arming_light();
|
||||
|
||||
#if COPTER_LEDS == ENABLED
|
||||
piezo_beep_twice();
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue