Skip to content
Snippets Groups Projects
Select Git revision
  • f1251d66f73015939baaa0a5ce4c862e3d0da268
  • master default protected
  • blink_multi_rocket
  • blink_rocket
  • msgctl/gfx_rle
  • msgctl/faultscreen
  • msgctl/textbuffer_api
  • schneider/bonding
  • schneider/bootloader-update-9a0d158
  • schneider/bsec
  • rahix/bma
  • rahix/bhi
  • schleicher-test
  • schneider/schleicher-test
  • freertos-btle
  • ch3/api-speed-eval2
  • schneider/mp-for-old-bl
  • ch3/leds-api
  • ch3/genapi-refactor
  • ch3/dual-core
  • dualcore
  • v0.0
22 results

utime.c

Blame
  • Forked from card10 / firmware
    Source project has a limited visibility.
    st3m_leds.c 8.04 KiB
    #include "st3m_leds.h"
    
    #include <math.h>
    #include <stdio.h>
    #include <string.h>
    
    #include "esp_log.h"
    #include "esp_system.h"
    #include "esp_task.h"
    #include "freertos/FreeRTOS.h"
    #include "freertos/semphr.h"
    #include "freertos/task.h"
    
    #include "flow3r_bsp.h"
    #include "st3m_colors.h"
    
    #define TAU360 0.017453292519943295
    
    static const char *TAG = "st3m-leds";
    
    typedef struct {
        uint8_t lut[256];
    } st3m_leds_gamma_table_t;
    
    typedef struct {
        uint16_t r;
        uint16_t g;
        uint16_t b;
    } st3m_u16_rgb_t;
    
    typedef struct {
        uint8_t r;
        uint8_t g;
        uint8_t b;
    } st3m_u8_rgb_t;
    
    typedef struct {
        uint8_t brightness;
        uint8_t slew_rate;
        bool auto_update;
        bool is_steady;
        uint8_t timer;
    
        st3m_leds_gamma_table_t gamma_red;
        st3m_leds_gamma_table_t gamma_green;
        st3m_leds_gamma_table_t gamma_blue;
    
        st3m_u8_rgb_t target[40];
        st3m_u8_rgb_t target_buffer[40];
        st3m_u16_rgb_t slew_output[40];
        st3m_u8_rgb_t ret_prev[40];
    } st3m_leds_state_t;
    
    static st3m_leds_state_t state;
    
    SemaphoreHandle_t mutex;
    #define LOCK xSemaphoreTake(mutex, portMAX_DELAY)
    #define UNLOCK xSemaphoreGive(mutex)
    
    SemaphoreHandle_t mutex_incoming;
    #define LOCK_INCOMING xSemaphoreTake(mutex_incoming, portMAX_DELAY)
    #define UNLOCK_INCOMING xSemaphoreGive(mutex_incoming)
    
    static void set_single_led(uint8_t index, st3m_u8_rgb_t c) {
        index = (index + 29) % 40;
        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) {
        new = new << 8;
        if (slew == 255 || (old == new)) return new;
        int16_t bonus = ((int16_t)slew) - 225;
        slew = 30 + (slew << 2) + ((slew * slew) >> 3);
        if (bonus > 0) {
            slew += 62 * bonus * bonus;
        }
    
        if (new > old + slew) {
            return old + slew;
        } else if (new > old) {
            return new;
        }
        if (new < old - slew) {
            return old - slew;
        } else if (new < old) {
            return new;
        }
        return old;
    }
    
    static void leds_update_target() {
        LOCK_INCOMING;
    
        for (int i = 0; i < 40; i++) {
            state.target[i].r = state.target_buffer[i].r;
            state.target[i].g = state.target_buffer[i].g;
            state.target[i].b = state.target_buffer[i].b;
        }
    
        UNLOCK_INCOMING;
    }
    
    void st3m_leds_update_hardware() {
        LOCK;
    
        if (state.auto_update) leds_update_target();
    
        bool is_different = false;
        bool is_steady = true;
    
        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))
                is_steady = false;
            state.slew_output[i] = c;
    
            c.r = ((uint32_t)c.r * state.brightness) >> 8;
            c.g = ((uint32_t)c.g * state.brightness) >> 8;
            c.b = ((uint32_t)c.b * state.brightness) >> 8;
    
            ret.r = state.gamma_red.lut[c.r >> 8];
            ret.g = state.gamma_red.lut[c.g >> 8];
            ret.b = state.gamma_red.lut[c.b >> 8];
    
            if ((ret.r != state.ret_prev[i].r) || (ret.g != state.ret_prev[i].g) ||
                (ret.b != state.ret_prev[i].b)) {
                set_single_led(i, ret);
                state.ret_prev[i] = ret;
                is_different = true;
            }
        }
        state.is_steady = is_steady;
        UNLOCK;
    
        if (is_different || (state.timer > 10)) {
            esp_err_t ret = flow3r_bsp_leds_refresh(portMAX_DELAY);
            if (ret != ESP_OK) {
                ESP_LOGE(TAG, "LED refresh failed: %s", esp_err_to_name(ret));
            }
            state.timer = 0;
        } else {
            state.timer += 1;
        }
    }
    
    void st3m_leds_set_single_rgba(uint8_t index, float red, float green,
                                   float blue, float alpha) {
        float r, g, b;
        st3m_leds_get_single_rgb(index, &r, &g, &b);
    
        float nalpha = 1 - alpha;
        r = alpha * red + nalpha * r;
        g = alpha * green + nalpha * g;
        b = alpha * blue + nalpha * blue;
        st3m_leds_set_single_rgb(index, r, g, b);
    }
    
    void st3m_leds_set_single_rgb(uint8_t index, float red, float green,
                                  float blue) {
        if (red > 1.0) red /= 255.0;
        if (green > 1.0) green /= 255.0;
        if (blue > 1.0) blue /= 255.0;
    
        LOCK_INCOMING;
        state.target_buffer[index].r = (uint8_t)(red * 255);
        state.target_buffer[index].g = (uint8_t)(green * 255);
        state.target_buffer[index].b = (uint8_t)(blue * 255);
        state.is_steady = false;
        UNLOCK_INCOMING;
    }
    
    void st3m_leds_get_single_rgb(uint8_t index, float *red, float *green,
                                  float *blue) {
        LOCK_INCOMING;
        *red = ((float)state.target_buffer[index].r) / 255;
        *green = ((float)state.target_buffer[index].g) / 255;
        *blue = ((float)state.target_buffer[index].b) / 255;
        UNLOCK_INCOMING;
    }
    
    void st3m_leds_set_single_hsv(uint8_t index, float hue, float sat, float val) {
        st3m_hsv_t hsv = {
            .h = hue * TAU360,
            .s = sat,
            .v = val,
        };
        st3m_rgb_t rgb = st3m_hsv_to_rgb(hsv);
        st3m_leds_set_single_rgb(index, rgb.r, rgb.g, rgb.b);
    }
    
    void st3m_leds_set_all_rgb(float red, float green, float blue) {
        for (int i = 0; i < 40; i++) {
            st3m_leds_set_single_rgb(i, red, green, blue);
        }
    }
    
    void st3m_leds_set_all_rgba(float red, float green, float blue, float alpha) {
        for (int i = 0; i < 40; i++) {
            st3m_leds_set_single_rgba(i, red, green, blue, alpha);
        }
    }
    
    void st3m_leds_set_all_hsv(float hue, float sat, float val) {
        st3m_hsv_t hsv = {
            .h = hue * TAU360,
            .s = sat,
            .v = val,
        };
        st3m_rgb_t rgb = st3m_hsv_to_rgb(hsv);
        for (int i = 0; i < 40; i++) {
            st3m_leds_set_single_rgb(i, rgb.r, rgb.g, rgb.b);
        }
    }
    
    void st3m_leds_update() { leds_update_target(); }
    
    static void _leds_task(void *_data) {
        (void)_data;
    
        TickType_t last_wake = xTaskGetTickCount();
        while (true) {
            vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(20));  // 50 Hz
            st3m_leds_update_hardware();
        }
    }
    
    void st3m_leds_init() {
        mutex = xSemaphoreCreateMutex();
        assert(mutex != NULL);
        mutex_incoming = xSemaphoreCreateMutex();
        assert(mutex_incoming != NULL);
    
        memset(&state, 0, sizeof(state));
        state.brightness = 70;
        state.slew_rate = 235;
        state.auto_update = false;
        state.timer = 255;
    
        for (uint16_t i = 0; i < 256; i++) {
            state.gamma_red.lut[i] = i;
            state.gamma_green.lut[i] = i;
            state.gamma_blue.lut[i] = i;
        }
    
        esp_err_t ret = flow3r_bsp_leds_init();
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "LED initialization failed: %s", esp_err_to_name(ret));
            abort();
        }
    
        xTaskCreate(_leds_task, "leds", 4096, NULL, ESP_TASK_PRIO_MIN + 4, NULL);
    
        ESP_LOGI(TAG, "LED task started");
    }
    
    void st3m_leds_set_brightness(uint8_t b) {
        LOCK;
        state.brightness = b;
        UNLOCK;
    }
    
    uint8_t st3m_leds_get_brightness() {
        LOCK;
        uint8_t res = state.brightness;
        UNLOCK;
        return res;
    }
    
    void st3m_leds_set_slew_rate(uint8_t s) {
        LOCK;
        state.slew_rate = s;
        UNLOCK;
    }
    
    uint8_t st3m_leds_get_slew_rate() {
        LOCK;
        uint8_t res = state.slew_rate;
        UNLOCK;
        return res;
    }
    
    bool st3m_leds_get_steady() {
        LOCK;
        uint8_t res = state.is_steady;
        UNLOCK;
        return res;
    }
    
    void st3m_leds_set_auto_update(bool on) {
        LOCK;
        state.auto_update = on;
        UNLOCK;
    }
    
    bool st3m_leds_get_auto_update() {
        LOCK;
        bool res = state.auto_update;
        UNLOCK;
        return res;
    }
    
    void st3m_leds_set_gamma(float red, float green, float blue) {
        LOCK;
        for (uint16_t i = 0; i < 256; i++) {
            if (i == 0) {
                state.gamma_red.lut[i] = 0;
                state.gamma_green.lut[i] = 0;
                state.gamma_blue.lut[i] = 0;
            }
            float step = ((float)i) / 255.;
            state.gamma_red.lut[i] = (uint8_t)(254. * (powf(step, red)) + 1);
            state.gamma_green.lut[i] = (uint8_t)(254. * (powf(step, green)) + 1);
            state.gamma_blue.lut[i] = (uint8_t)(254. * (powf(step, blue)) + 1);
        }
        UNLOCK;
    }