diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 3f316c7628..a872f03f7d 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -49,6 +49,13 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = { // @User: Advanced AP_GROUPINFO("BUZZ_ENABLE", 1, AP_Notify, _buzzer_enable, BUZZER_ON), + // @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 + // @User: Advanced + AP_GROUPINFO("LED_OVERRIDE", 2, AP_Notify, _rgb_led_override, 0), + AP_GROUPEND }; diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 18663119b0..5055c4e400 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -114,5 +114,6 @@ private: static NotifyDevice* _devices[]; AP_Int8 _rgb_led_brightness; + AP_Int8 _rgb_led_override; AP_Int8 _buzzer_enable; }; diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp index b3fa48807d..363a602b7c 100644 --- a/libraries/AP_Notify/RGBLed.cpp +++ b/libraries/AP_Notify/RGBLed.cpp @@ -48,13 +48,8 @@ bool RGBLed::init() } // 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) +void RGBLed::_set_rgb(uint8_t red, uint8_t green, uint8_t blue) { - // return immediately if not enabled - if (!_healthy) { - return; - } - if (red != _red_curr || green != _green_curr || blue != _blue_curr) { @@ -67,6 +62,20 @@ void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue) } } +// 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) +{ + // return immediately if not enabled + if (!_healthy) { + return; + } + if (pNotify->_rgb_led_override) { + // don't set if in override mode + return; + } + _set_rgb(red, green, blue); +} + // _scheduled_update - updates _red, _green, _blue according to notify flags void RGBLed::update_colours(void) @@ -323,6 +332,66 @@ void RGBLed::update() if (!_healthy) { return; } - update_colours(); - set_rgb(_red_des, _green_des, _blue_des); + if (!pNotify->_rgb_led_override) { + update_colours(); + set_rgb(_red_des, _green_des, _blue_des); + } else { + update_override(); + } +} + +/* + handle LED control, only used when LED_OVERRIDE=1 +*/ +void RGBLed::handle_led_control(mavlink_message_t *msg) +{ + if (!pNotify->_rgb_led_override) { + // ignore LED_CONTROL commands if not in LED_OVERRIDE mode + return; + } + + // decode mavlink message + mavlink_led_control_t packet; + mavlink_msg_led_control_decode(msg, &packet); + + _led_override.start_ms = AP_HAL::millis(); + + switch (packet.custom_len) { + case 3: + _led_override.rate_hz = 0; + _led_override.r = packet.custom_bytes[0]; + _led_override.g = packet.custom_bytes[1]; + _led_override.b = packet.custom_bytes[2]; + break; + case 4: + _led_override.rate_hz = packet.custom_bytes[3]; + _led_override.r = packet.custom_bytes[0]; + _led_override.g = packet.custom_bytes[1]; + _led_override.b = packet.custom_bytes[2]; + break; + default: + // not understood + break; + } +} + +/* + update LED when in override mode + */ +void RGBLed::update_override(void) +{ + if (_led_override.rate_hz == 0) { + // solid colour + _set_rgb(_led_override.r, _led_override.g, _led_override.b); + return; + } + // blinking + uint32_t ms_per_cycle = 1000 / _led_override.rate_hz; + uint32_t cycle = (AP_HAL::millis() - _led_override.start_ms) % ms_per_cycle; + if (cycle > ms_per_cycle / 2) { + // on + _set_rgb(_led_override.r, _led_override.g, _led_override.b); + } else { + _set_rgb(0, 0, 0); + } } diff --git a/libraries/AP_Notify/RGBLed.h b/libraries/AP_Notify/RGBLed.h index 5879a12fd3..d070c5216d 100644 --- a/libraries/AP_Notify/RGBLed.h +++ b/libraries/AP_Notify/RGBLed.h @@ -39,11 +39,19 @@ public: // called at 50Hz virtual void update(); + // handle LED control, only used when LED_OVERRIDE=1 + virtual void handle_led_control(mavlink_message_t *msg) override; + protected: // methods implemented in hardware specific classes virtual bool hw_init(void) = 0; virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) = 0; + // 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(); + // meta-data common to all hw devices uint8_t counter; uint8_t step; @@ -54,6 +62,13 @@ protected: uint8_t _led_bright; uint8_t _led_medium; uint8_t _led_dim; + + struct { + uint8_t r, g, b; + uint8_t rate_hz; + uint32_t start_ms; + } _led_override; + private: virtual void update_colours(); };