add controller logic

Src/cyphal_application/lights/lights.cpp

@@ -7,20 +7,16 @@
 #include "cyphal.hpp"
 #include "params.hpp"
 #include "main.h"
-#include "periphery/ws2812/ws2812.h"
 #include "periphery/adc/adc.hpp"
 
+#define WS2812_LEDS_AMOUNT 32
 extern TIM_HandleTypeDef htim2;
 
-static constexpr uint8_t RED_SCALE = 256 / (reg_udral_physics_optics_HighColor_0_1_MAX_RED + 1);
-static constexpr uint8_t GREEN_SCALE = 256 / (reg_udral_physics_optics_HighColor_0_1_MAX_GREEN + 1);
-static constexpr uint8_t BLUE_SCALE = 256 / (reg_udral_physics_optics_HighColor_0_1_MAX_BLUE + 1);
-
 
 int8_t RgbLights::init() {
     int8_t res;
 
-    res = ws2812bInit(32, &htim2, TIM_CHANNEL_3);
+    res = ws2812bInit(WS2812_LEDS_AMOUNT, &htim2, TIM_CHANNEL_3);
     if (res < 0) {
         return res;
     }
@@ -39,33 +35,65 @@ int8_t RgbLights::init() {
 };
 
 void RgbLights::update() {
-    static uint32_t next_time_ms = 0;
-    if (HAL_GetTick() < next_time_ms) {
+    _process_crct_remperature();
+    _process_lights();
+}
+
+/**
+ * @note
+ * The temperture formula is valid only for stm32f103
+ * We can measure ADC much faster and a filter can be added here
+ */
+void RgbLights::_process_crct_remperature() {
+    if (HAL_GetTick() < _next_crct_process_time_ms) {
         return;
     }
-    next_time_ms = HAL_GetTick() + 10;
+    _next_crct_process_time_ms = HAL_GetTick() + 250;
 
-    // Temperature
     static const uint16_t TEMP_REF = 25;
     static const uint16_t ADC_REF = 1750;   ///< v_ref / 3.3 * 4095
     static const uint16_t AVG_SLOPE = 5;    ///< avg_slope/(3.3/4096)
     _temp_pub.setPortId(paramsGetIntegerValue(PARAM_CRCT_TEMPERATURE_ID));
     if (_temp_pub.isEnabled()) {
         uint16_t adc_measurement = AdcPeriphery::get(AdcChannel::ADC_TEMPERATURE);
+        current_temperature = (ADC_REF - adc_measurement) / AVG_SLOPE + TEMP_REF + 273;
         uavcan_si_sample_temperature_Scalar_1_0 msg;
-        msg.kelvin = (ADC_REF - adc_measurement) / AVG_SLOPE + TEMP_REF + 273;
+        msg.kelvin = current_temperature;
         _temp_pub.publish(msg);
     }
+}
+
+/**
+ * @note
+ * 25 Hz blinking is considered to be enough.
+ * Start decreasing the brighness when temperature is higher than 60 °C.
+ * Do not allow to reach 80 °C.
+ */
+void RgbLights::_process_lights() {
+    if (HAL_GetTick() < _next_light_process_time_ms) {
+        return;
+    }
+    _next_light_process_time_ms = HAL_GetTick() + 20;
 
-    // Lights
     auto color = _rgbled_sub.get();
-    static Leds_Color_t leds;
 
-    for (uint_fast8_t led_idx = 0; led_idx < 32; led_idx++) {
-        leds.colors[led_idx].shades.red = color.red * RED_SCALE;
-        leds.colors[led_idx].shades.green = color.green * GREEN_SCALE;
-        leds.colors[led_idx].shades.blue = color.blue * BLUE_SCALE;
+    float max_brightness;
+    if (current_temperature <= CONTROL_MIN) {
+        max_brightness = 1.0;
+    } else if (current_temperature >= CONTROL_MAX) {
+        max_brightness = 0.0;
+    } else {
+        max_brightness = 1.0f - (current_temperature - CONTROL_MIN) / (CONTROL_MAX - CONTROL_MIN);
+    }
+
+    uint8_t red = (float)color.red / reg_udral_physics_optics_HighColor_0_1_MAX_RED * 255.0f * max_brightness;
+    uint8_t green = (float)color.green / reg_udral_physics_optics_HighColor_0_1_MAX_GREEN * 255.0f * max_brightness;
+    uint8_t blue = (float)color.blue / reg_udral_physics_optics_HighColor_0_1_MAX_BLUE * 255.0f * max_brightness;
+    for (uint_fast8_t led_idx = 0; led_idx < WS2812_LEDS_AMOUNT; led_idx++) {
+        _leds.colors[led_idx].shades.red = red;
+        _leds.colors[led_idx].shades.green = green;
+        _leds.colors[led_idx].shades.blue = blue;
     }
-    ws2812bSetColors(&leds);
+    ws2812bSetColors(&_leds);
     ws2812bStartOnce();
 }

Src/cyphal_application/lights/lights.hpp

@@ -8,16 +8,31 @@
 #include "cyphal_subscribers.hpp"
 #include "Udral/rgbled.hpp"
 #include "Udral/circuit_status.hpp"
+#include "periphery/ws2812/ws2812.h"
 
 class RgbLights {
 public:
     RgbLights(cyphal::Cyphal* driver) : _rgbled_sub(driver), _temp_pub(driver, 0) {};
     int8_t init();
     void update();
 private:
-
+    void _process_lights();
     cyphal::HighColorSubscriber _rgbled_sub;
+    Leds_Color_t _leds;
+    uint32_t _next_light_process_time_ms{0};
+
+    void _process_crct_remperature();
     RaccoonLab::CircuitStatusTemperaturePublisher _temp_pub;
+    float current_temperature;
+    uint32_t _next_crct_process_time_ms{0};
+
+    /**
+     * @brief Controller logic
+     * If the temperature goes above 60 °C, the controller will decrease the maximum brightness.
+     * If the temperature goes above 80 °C, the controller will turn off the lights.
+     */
+    static constexpr const float CONTROL_MIN = 273.15 + 60;
+    static constexpr const float CONTROL_MAX = 273.15 + 80;
 };
 
 #endif  // SRC_CYPHAL_APPLICATION_LIGHTS_LIGHTS_HPP_