diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 231ca80660..0a6283a2a8 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -111,8 +111,8 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = { // @Param: LED_OVERRIDE // @DisplayName: Specifies colour source for the RGBLed - // @Description: Specifies the source for the colours and brightness for the LED. OutbackChallenge conforms to the MedicalExpress (https://uavchallenge.org/medical-express/) rules, essentially "Green" is disarmed (safe-to-approach), "Red" is armed (not safe-to-approach). - // @Values: 0:Standard,1:MAVLink,2:OutbackChallenge + // @Description: Specifies the source for the colours and brightness for the LED. OutbackChallenge conforms to the MedicalExpress (https://uavchallenge.org/medical-express/) rules, essentially "Green" is disarmed (safe-to-approach), "Red" is armed (not safe-to-approach). Traffic light is a simplified color set, red when armed, yellow when the safety switch is not surpressing outputs (but disarmed), and green when outputs are surpressed and disarmed, the LED will blink faster if disarmed and failing arming checks. + // @Values: 0:Standard,1:MAVLink,2:OutbackChallenge,3:TrafficLight // @User: Advanced AP_GROUPINFO("LED_OVERRIDE", 2, AP_Notify, _rgb_led_override, 0), diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp index c0db86c674..a20c17d6fa 100644 --- a/libraries/AP_Notify/RGBLed.cpp +++ b/libraries/AP_Notify/RGBLed.cpp @@ -162,6 +162,30 @@ uint32_t RGBLed::get_colour_sequence(void) const return sequence_disarmed_bad_gps; } +uint32_t RGBLed::get_colour_sequence_traffic_light(void) const +{ + if (AP_Notify::flags.initialising) { + return DEFINE_COLOUR_SEQUENCE(RED,GREEN,BLUE,RED,GREEN,BLUE,RED,GREEN,BLUE,OFF); + } + + if (AP_Notify::flags.armed) { + return DEFINE_COLOUR_SEQUENCE_SLOW(RED); + } + + if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { + if (!AP_Notify::flags.pre_arm_check) { + return DEFINE_COLOUR_SEQUENCE_ALTERNATE(YELLOW, OFF); + } else { + return DEFINE_COLOUR_SEQUENCE_SLOW(YELLOW); + } + } + + if (!AP_Notify::flags.pre_arm_check) { + return DEFINE_COLOUR_SEQUENCE_ALTERNATE(GREEN, OFF); + } + return DEFINE_COLOUR_SEQUENCE_SLOW(GREEN); +} + // update - updates led according to timed_updated. Should be called // at 50Hz void RGBLed::update() @@ -178,6 +202,9 @@ void RGBLed::update() case obc: current_colour_sequence = get_colour_sequence_obc(); break; + case traffic_light: + current_colour_sequence = get_colour_sequence_traffic_light(); + break; } const uint8_t brightness = get_brightness(); diff --git a/libraries/AP_Notify/RGBLed.h b/libraries/AP_Notify/RGBLed.h index e803a7cfa0..6b4bb54bc7 100644 --- a/libraries/AP_Notify/RGBLed.h +++ b/libraries/AP_Notify/RGBLed.h @@ -67,6 +67,7 @@ private: void update_colours(); uint32_t get_colour_sequence() const; uint32_t get_colour_sequence_obc() const; + uint32_t get_colour_sequence_traffic_light() const; uint8_t get_brightness(void) const; @@ -108,6 +109,7 @@ private: standard = 0, mavlink = 1, obc = 2, + traffic_light = 3, }; rgb_source_t rgb_source() const;