mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_Notify: tidy and remove code duplications
Saves 60 bytes, too
This commit is contained in:
Peter Barker
Peter Barker
parent
6daa241235
commit
df836ec70c
@ -62,9 +62,7 @@ void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
|
||||
_set_rgb(red, green, blue);
|
||||
}
|
||||
|
||||
|
||||
// _scheduled_update - updates _red, _green, _blue according to notify flags
|
||||
void RGBLed::update_colours(void)
|
||||
uint8_t RGBLed::get_brightness(void) const
|
||||
{
|
||||
uint8_t brightness = _led_bright;
|
||||
|
||||
@ -83,228 +81,88 @@ void RGBLed::update_colours(void)
|
||||
break;
|
||||
}
|
||||
|
||||
const uint8_t step = (AP_HAL::millis()/100) % 10;
|
||||
|
||||
// use dim light when connected through USB
|
||||
if (hal.gpio->usb_connected() && brightness > _led_dim) {
|
||||
brightness = _led_dim;
|
||||
}
|
||||
|
||||
// initialising pattern
|
||||
if (AP_Notify::flags.initialising) {
|
||||
if (step & 1) {
|
||||
// odd steps display red light
|
||||
_red_des = brightness;
|
||||
_blue_des = _led_off;
|
||||
_green_des = _led_off;
|
||||
} else {
|
||||
// even display blue light
|
||||
_red_des = _led_off;
|
||||
_blue_des = brightness;
|
||||
_green_des = _led_off;
|
||||
return brightness;
|
||||
}
|
||||
|
||||
// exit so no other status modify this pattern
|
||||
return;
|
||||
// _scheduled_update - updates _red, _green, _blue according to notify flags
|
||||
uint32_t RGBLed::get_colour_sequence(void) const
|
||||
{
|
||||
// initialising pattern
|
||||
if (AP_Notify::flags.initialising) {
|
||||
return sequence_initialising;
|
||||
}
|
||||
|
||||
// save trim and esc calibration pattern
|
||||
if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
|
||||
switch(step) {
|
||||
case 0:
|
||||
case 3:
|
||||
case 6:
|
||||
// red on
|
||||
_red_des = brightness;
|
||||
_blue_des = _led_off;
|
||||
_green_des = _led_off;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
case 4:
|
||||
case 7:
|
||||
// blue on
|
||||
_red_des = _led_off;
|
||||
_blue_des = brightness;
|
||||
_green_des = _led_off;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
case 5:
|
||||
case 8:
|
||||
// green on
|
||||
_red_des = _led_off;
|
||||
_blue_des = _led_off;
|
||||
_green_des = brightness;
|
||||
break;
|
||||
|
||||
case 9:
|
||||
// all off
|
||||
_red_des = _led_off;
|
||||
_blue_des = _led_off;
|
||||
_green_des = _led_off;
|
||||
break;
|
||||
}
|
||||
// exit so no other status modify this pattern
|
||||
return;
|
||||
return sequence_trim_or_esc;
|
||||
}
|
||||
|
||||
// radio and battery failsafe patter: flash yellow
|
||||
// gps failsafe pattern : flashing yellow and blue
|
||||
// ekf_bad pattern : flashing yellow and red
|
||||
if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery ||
|
||||
AP_Notify::flags.ekf_bad || AP_Notify::flags.gps_glitching || AP_Notify::flags.leak_detected) {
|
||||
switch(step) {
|
||||
case 0:
|
||||
case 1:
|
||||
case 2:
|
||||
case 3:
|
||||
case 4:
|
||||
// yellow on
|
||||
_red_des = brightness;
|
||||
_blue_des = _led_off;
|
||||
_green_des = brightness;
|
||||
break;
|
||||
case 5:
|
||||
case 6:
|
||||
case 7:
|
||||
case 8:
|
||||
case 9:
|
||||
if (AP_Notify::flags.failsafe_radio ||
|
||||
AP_Notify::flags.failsafe_battery ||
|
||||
AP_Notify::flags.ekf_bad ||
|
||||
AP_Notify::flags.gps_glitching ||
|
||||
AP_Notify::flags.leak_detected) {
|
||||
|
||||
if (AP_Notify::flags.leak_detected) {
|
||||
// purple if leak detected
|
||||
_red_des = brightness;
|
||||
_blue_des = brightness;
|
||||
_green_des = brightness;
|
||||
return sequence_failsafe_leak;
|
||||
} else if (AP_Notify::flags.ekf_bad) {
|
||||
// red on if ekf bad
|
||||
_red_des = brightness;
|
||||
_blue_des = _led_off;
|
||||
_green_des = _led_off;
|
||||
return sequence_failsafe_ekf;
|
||||
} else if (AP_Notify::flags.gps_glitching) {
|
||||
// blue on gps glitch
|
||||
_red_des = _led_off;
|
||||
_blue_des = brightness;
|
||||
_green_des = _led_off;
|
||||
}else{
|
||||
return sequence_failsafe_gps_glitching;
|
||||
}
|
||||
// all off for radio or battery failsafe
|
||||
_red_des = _led_off;
|
||||
_blue_des = _led_off;
|
||||
_green_des = _led_off;
|
||||
}
|
||||
break;
|
||||
}
|
||||
// exit so no other status modify this pattern
|
||||
return;
|
||||
return sequence_failsafe_radio_or_battery;
|
||||
}
|
||||
|
||||
// solid green or blue if armed
|
||||
if (AP_Notify::flags.armed) {
|
||||
// solid green if armed with GPS 3d lock
|
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
_red_des = _led_off;
|
||||
_blue_des = _led_off;
|
||||
_green_des = brightness;
|
||||
}else{
|
||||
// solid blue if armed with no GPS lock
|
||||
_red_des = _led_off;
|
||||
_blue_des = brightness;
|
||||
_green_des = _led_off;
|
||||
return sequence_armed;
|
||||
}
|
||||
return;
|
||||
}else{
|
||||
// solid blue if armed with no GPS lock
|
||||
return sequence_armed_nogps;
|
||||
}
|
||||
|
||||
// double flash yellow if failing pre-arm checks
|
||||
if (!AP_Notify::flags.pre_arm_check) {
|
||||
switch(step) {
|
||||
case 0:
|
||||
case 1:
|
||||
case 4:
|
||||
case 5:
|
||||
// yellow on
|
||||
_red_des = brightness;
|
||||
_blue_des = _led_off;
|
||||
_green_des = brightness;
|
||||
break;
|
||||
case 2:
|
||||
case 3:
|
||||
case 6:
|
||||
case 7:
|
||||
case 8:
|
||||
case 9:
|
||||
// all off
|
||||
_red_des = _led_off;
|
||||
_blue_des = _led_off;
|
||||
_green_des = _led_off;
|
||||
break;
|
||||
return sequence_prearm_failing;
|
||||
}
|
||||
}else{
|
||||
// fast flashing green if disarmed with GPS 3D lock and DGPS
|
||||
// slow flashing green if disarmed with GPS 3d lock (and no DGPS)
|
||||
// flashing blue if disarmed with no gps lock or gps pre_arm checks have failed
|
||||
bool fast_green = AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check;
|
||||
switch(step) {
|
||||
case 0:
|
||||
if (fast_green) {
|
||||
_green_des = brightness;
|
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check) {
|
||||
return sequence_disarmed_good_dgps;
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
if (fast_green) {
|
||||
_green_des = _led_off;
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
if (fast_green) {
|
||||
_green_des = brightness;
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
if (fast_green) {
|
||||
_green_des = _led_off;
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
_red_des = _led_off;
|
||||
|
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && AP_Notify::flags.pre_arm_gps_check) {
|
||||
// flashing green if disarmed with GPS 3d lock
|
||||
_blue_des = _led_off;
|
||||
_green_des = brightness;
|
||||
}else{
|
||||
// flashing blue if disarmed with no gps lock
|
||||
_blue_des = brightness;
|
||||
_green_des = _led_off;
|
||||
return sequence_disarmed_good_gps;
|
||||
}
|
||||
break;
|
||||
case 5:
|
||||
if (fast_green) {
|
||||
_green_des = _led_off;
|
||||
}
|
||||
break;
|
||||
|
||||
case 6:
|
||||
if (fast_green) {
|
||||
_green_des = brightness;
|
||||
return sequence_disarmed_bad_gps;
|
||||
}
|
||||
break;
|
||||
|
||||
case 7:
|
||||
if (fast_green) {
|
||||
_green_des = _led_off;
|
||||
}
|
||||
break;
|
||||
case 8:
|
||||
if (fast_green) {
|
||||
_green_des = brightness;
|
||||
}
|
||||
break;
|
||||
case 9:
|
||||
// all off
|
||||
_red_des = _led_off;
|
||||
_blue_des = _led_off;
|
||||
_green_des = _led_off;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
// _scheduled_update - updates _red, _green, _blue according to notify flags
|
||||
void RGBLed::update_colours(void)
|
||||
{
|
||||
const uint8_t brightness = get_brightness();
|
||||
|
||||
const uint32_t current_colour_sequence = get_colour_sequence();
|
||||
|
||||
const uint8_t step = (AP_HAL::millis()/100) % 10;
|
||||
|
||||
const uint8_t colour = current_colour_sequence >> (step*3);
|
||||
|
||||
_red_des = (colour & RED) ? brightness : 0;
|
||||
_green_des = (colour & GREEN) ? brightness : 0;
|
||||
_blue_des = (colour & BLUE) ? brightness : 0;
|
||||
}
|
||||
|
||||
// update - updates led according to timed_updated. Should be called
|
||||
|
||||
@ -64,5 +64,42 @@ protected:
|
||||
} _led_override;
|
||||
|
||||
private:
|
||||
virtual void update_colours();
|
||||
void update_colours();
|
||||
uint32_t get_colour_sequence() const;
|
||||
|
||||
uint8_t get_brightness(void) const;
|
||||
|
||||
#define DEFINE_COLOUR_SEQUENCE(S0, S1, S2, S3, S4, S5, S6, S7, S8, S9) \
|
||||
((S0) << (0*3) | (S1) << (1*3) | (S2) << (2*3) | (S3) << (3*3) | (S4) << (4*3) | (S5) << (5*3) | (S6) << (6*3) | (S7) << (7*3) | (S8) << (8*3) | (S9) << (9*3))
|
||||
|
||||
#define DEFINE_COLOUR_SEQUENCE_SLOW(colour) \
|
||||
DEFINE_COLOUR_SEQUENCE(colour,colour,colour,colour,colour,OFF,OFF,OFF,OFF,OFF)
|
||||
#define DEFINE_COLOUR_SEQUENCE_FAILSAFE(colour) \
|
||||
DEFINE_COLOUR_SEQUENCE(YELLOW,YELLOW,YELLOW,YELLOW,YELLOW,colour,colour,colour,colour,colour)
|
||||
#define DEFINE_COLOUR_SEQUENCE_SOLID(colour) \
|
||||
DEFINE_COLOUR_SEQUENCE(colour,colour,colour,colour,colour,colour,colour,colour,colour,colour)
|
||||
#define DEFINE_COLOUR_SEQUENCE_ALTERNATE(colour1, colour2) \
|
||||
DEFINE_COLOUR_SEQUENCE(colour1,colour2,colour1,colour2,colour1,colour2,colour1,colour2,colour1,colour2)
|
||||
|
||||
#define OFF 0
|
||||
#define BLUE 1
|
||||
#define GREEN 2
|
||||
#define RED 4
|
||||
#define YELLOW (RED|GREEN)
|
||||
#define WHITE (RED|GREEN|BLUE)
|
||||
|
||||
const uint32_t sequence_initialising = DEFINE_COLOUR_SEQUENCE_ALTERNATE(RED,BLUE);
|
||||
const uint32_t sequence_trim_or_esc = DEFINE_COLOUR_SEQUENCE(RED,BLUE,GREEN,RED,BLUE,GREEN,RED,BLUE,GREEN,OFF);
|
||||
const uint32_t sequence_failsafe_leak = DEFINE_COLOUR_SEQUENCE_FAILSAFE(WHITE);
|
||||
const uint32_t sequence_failsafe_ekf = DEFINE_COLOUR_SEQUENCE_FAILSAFE(RED);
|
||||
const uint32_t sequence_failsafe_gps_glitching = DEFINE_COLOUR_SEQUENCE_FAILSAFE(BLUE);
|
||||
const uint32_t sequence_failsafe_radio_or_battery = DEFINE_COLOUR_SEQUENCE_FAILSAFE(OFF);
|
||||
|
||||
const uint32_t sequence_armed = DEFINE_COLOUR_SEQUENCE_SOLID(GREEN);
|
||||
const uint32_t sequence_armed_nogps = DEFINE_COLOUR_SEQUENCE_SOLID(BLUE);
|
||||
const uint32_t sequence_prearm_failing = DEFINE_COLOUR_SEQUENCE(YELLOW,YELLOW,OFF,OFF,YELLOW,YELLOW,OFF,OFF,OFF,OFF);
|
||||
const uint32_t sequence_disarmed_good_dgps = DEFINE_COLOUR_SEQUENCE_ALTERNATE(GREEN,OFF);
|
||||
const uint32_t sequence_disarmed_good_gps = DEFINE_COLOUR_SEQUENCE_SLOW(GREEN);
|
||||
const uint32_t sequence_disarmed_bad_gps = DEFINE_COLOUR_SEQUENCE_SLOW(BLUE);
|
||||
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user