diff --git a/libraries/AP_Notify/RCOutputRGBLed.cpp b/libraries/AP_Notify/RCOutputRGBLed.cpp index 41d8b088cb..a52d03edce 100644 --- a/libraries/AP_Notify/RCOutputRGBLed.cpp +++ b/libraries/AP_Notify/RCOutputRGBLed.cpp @@ -53,6 +53,17 @@ bool RCOutputRGBLed::hw_init() return true; } +uint16_t RCOutputRGBLed::get_duty_cycle_for_color(const uint8_t color, const uint16_t usec_period) const +{ + return usec_period * color / _led_bright; +} + +uint16_t RCOutputRGBLedInverted::get_duty_cycle_for_color(const uint8_t color, const uint16_t usec_period) const +{ + return usec_period * (255 - color) / _led_bright; +} + + bool RCOutputRGBLed::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) { const uint16_t freq_motor = hal.rcout->get_freq(0); @@ -69,13 +80,13 @@ bool RCOutputRGBLed::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) hal.rcout->set_freq(mask, freq_motor); } - uint16_t usec_duty = usec_period * red / _led_bright; + uint16_t usec_duty = get_duty_cycle_for_color(red, usec_period); SRV_Channels::set_output_pwm_chan(_red_channel, usec_duty); - usec_duty = usec_period * green / _led_bright; + usec_duty = get_duty_cycle_for_color(green, usec_period); SRV_Channels::set_output_pwm_chan(_green_channel, usec_duty); - usec_duty = usec_period * blue / _led_bright; + usec_duty = get_duty_cycle_for_color(blue, usec_period); SRV_Channels::set_output_pwm_chan(_blue_channel, usec_duty); return true; diff --git a/libraries/AP_Notify/RCOutputRGBLed.h b/libraries/AP_Notify/RCOutputRGBLed.h index 38eb0866ed..39522f161a 100644 --- a/libraries/AP_Notify/RCOutputRGBLed.h +++ b/libraries/AP_Notify/RCOutputRGBLed.h @@ -13,6 +13,7 @@ public: protected: bool hw_init() override; virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) override; + virtual uint16_t get_duty_cycle_for_color(const uint8_t color, const uint16_t usec_period) const; private: uint8_t _red_channel; @@ -20,6 +21,15 @@ private: uint8_t _blue_channel; }; +class RCOutputRGBLedInverted : public RCOutputRGBLed { +public: + RCOutputRGBLedInverted(uint8_t red_channel, uint8_t green_channel, uint8_t blue_channel) + : RCOutputRGBLed(red_channel, green_channel, blue_channel) + { } +protected: + virtual uint16_t get_duty_cycle_for_color(const uint8_t color, const uint16_t usec_period) const override; +}; + class RCOutputRGBLedOff : public RCOutputRGBLed { public: RCOutputRGBLedOff(uint8_t red_channel, uint8_t green_channel,