diff --git a/components/st3m/st3m_leds.c b/components/st3m/st3m_leds.c index fedac5d3c4618712e299eb29c8dc49936e3b0575..85b379e78c37f87dfc739b041951717343cd1b2f 100644 --- a/components/st3m/st3m_leds.c +++ b/components/st3m/st3m_leds.c @@ -16,6 +16,8 @@ #define TAU360 0.017453292519943295 +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) + static const char *TAG = "st3m-leds"; typedef struct { @@ -66,7 +68,8 @@ static void set_single_led(uint8_t index, st3m_u8_rgb_t c) { flow3r_bsp_leds_set_pixel(index, c.r, c.g, c.b); } -static uint16_t led_get_slew(uint16_t old, uint16_t new, uint16_t slew) { +static uint16_t led_get_slew(uint16_t old, uint16_t new, uint16_t slew, + uint16_t factor) { new = new << 8; if (slew == 255 || (old == new)) return new; int16_t bonus = ((int16_t)slew) - 225; @@ -74,6 +77,7 @@ static uint16_t led_get_slew(uint16_t old, uint16_t new, uint16_t slew) { if (bonus > 0) { slew += 62 * bonus * bonus; } + slew = ((uint32_t)slew * factor) >> 8; if (new > old + slew) { return old + slew; @@ -110,13 +114,21 @@ void st3m_leds_update_hardware() { for (int i = 0; i < 40; i++) { st3m_u8_rgb_t ret = state.target[i]; - st3m_u16_rgb_t c; st3m_u16_rgb_t prev = state.slew_output[i]; - c.r = led_get_slew(prev.r, ret.r, state.slew_rate); - c.g = led_get_slew(prev.g, ret.g, state.slew_rate); - c.b = led_get_slew(prev.b, ret.b, state.slew_rate); - if ((prev.r != c.r) || (prev.g != c.g) || (prev.b != c.b)) + st3m_u16_rgb_t c = prev; + uint16_t diff_r = abs((int32_t)prev.r - (ret.r << 8u)); + uint16_t diff_g = abs((int32_t)prev.g - (ret.g << 8u)); + uint16_t diff_b = abs((int32_t)prev.b - (ret.b << 8u)); + uint16_t max_diff = MAX(MAX(diff_r, diff_g), diff_b); + if (max_diff) { + c.r = led_get_slew(prev.r, ret.r, state.slew_rate, + ((uint32_t)diff_r * 256) / max_diff); + c.g = led_get_slew(prev.g, ret.g, state.slew_rate, + ((uint32_t)diff_g * 256) / max_diff); + c.b = led_get_slew(prev.b, ret.b, state.slew_rate, + ((uint32_t)diff_b * 256) / max_diff); is_steady = false; + } state.slew_output[i] = c; c.r = ((uint32_t)c.r * state.brightness) >> 8;