diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index e3170d8d20..90b5d719df 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -109,9 +109,9 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = { AP_GROUPINFO("BUZZ_ENABLE", 1, AP_Notify, _buzzer_enable, BUZZER_ENABLE_DEFAULT), // @Param: LED_OVERRIDE - // @DisplayName: Setup for MAVLink LED override - // @Description: This sets up the board RGB LED for override by MAVLink. Normal notify LED control is disabled - // @Values: 0:Disable,1:Enable + // @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 // @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 9cbd8ec656..0f77351169 100644 --- a/libraries/AP_Notify/RGBLed.cpp +++ b/libraries/AP_Notify/RGBLed.cpp @@ -52,10 +52,15 @@ void RGBLed::_set_rgb(uint8_t red, uint8_t green, uint8_t blue) } } +RGBLed::rgb_source_t RGBLed::rgb_source() const +{ + return rgb_source_t(pNotify->_rgb_led_override.get()); +} + // set_rgb - set color as a combination of red, green and blue values void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue) { - if (pNotify->_rgb_led_override) { + if (rgb_source() == mavlink) { // don't set if in override mode return; } @@ -88,6 +93,14 @@ uint8_t RGBLed::get_brightness(void) const return brightness; } +uint32_t RGBLed::get_colour_sequence_obc(void) const +{ + if (AP_Notify::flags.armed) { + return DEFINE_COLOUR_SEQUENCE_SOLID(RED); + } + return DEFINE_COLOUR_SEQUENCE_SOLID(GREEN); +} + // _scheduled_update - updates _red, _green, _blue according to notify flags uint32_t RGBLed::get_colour_sequence(void) const { @@ -149,12 +162,25 @@ uint32_t RGBLed::get_colour_sequence(void) const return sequence_disarmed_bad_gps; } -// _scheduled_update - updates _red, _green, _blue according to notify flags -void RGBLed::update_colours(void) +// update - updates led according to timed_updated. Should be called +// at 50Hz +void RGBLed::update() { - const uint8_t brightness = get_brightness(); + uint32_t current_colour_sequence = 0; - const uint32_t current_colour_sequence = get_colour_sequence(); + switch (rgb_source()) { + case mavlink: + update_override(); + return; // note this is a return not a break! + case standard: + current_colour_sequence = get_colour_sequence(); + break; + case obc: + current_colour_sequence = get_colour_sequence_obc(); + break; + } + + const uint8_t brightness = get_brightness(); uint8_t step = (AP_HAL::millis()/100) % 10; @@ -169,18 +195,8 @@ void RGBLed::update_colours(void) _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 -// at 50Hz -void RGBLed::update() -{ - if (!pNotify->_rgb_led_override) { - update_colours(); - set_rgb(_red_des, _green_des, _blue_des); - } else { - update_override(); - } + set_rgb(_red_des, _green_des, _blue_des); } /* @@ -188,7 +204,7 @@ void RGBLed::update() */ void RGBLed::handle_led_control(mavlink_message_t *msg) { - if (!pNotify->_rgb_led_override) { + if (rgb_source() == mavlink) { // ignore LED_CONTROL commands if not in LED_OVERRIDE mode return; } diff --git a/libraries/AP_Notify/RGBLed.h b/libraries/AP_Notify/RGBLed.h index ed0bb09fa0..66418cf9a4 100644 --- a/libraries/AP_Notify/RGBLed.h +++ b/libraries/AP_Notify/RGBLed.h @@ -47,7 +47,7 @@ protected: // set_rgb - set color as a combination of red, green and blue levels from 0 ~ 15 virtual void _set_rgb(uint8_t red, uint8_t green, uint8_t blue); - virtual void update_override(); + void update_override(); // meta-data common to all hw devices uint8_t _red_des, _green_des, _blue_des; // color requested by timed update @@ -66,6 +66,7 @@ protected: private: void update_colours(); uint32_t get_colour_sequence() const; + uint32_t get_colour_sequence_obc() const; uint8_t get_brightness(void) const; @@ -103,4 +104,11 @@ private: const uint32_t sequence_disarmed_bad_gps = DEFINE_COLOUR_SEQUENCE_SLOW(BLUE); uint8_t last_step; + enum rgb_source_t { + standard = 0, + mavlink = 1, + obc = 2, + }; + rgb_source_t rgb_source() const; + };