From 5fd3b5a2db226e35e768b70659463085850b0697 Mon Sep 17 00:00:00 2001
From: PonomarevDA <ponomarevda96@gmail.com>
Date: Tue, 26 Dec 2023 01:19:42 +0300
Subject: [PATCH] add controller logic

---
 Src/cyphal_application/lights/lights.cpp | 64 +++++++++++++++++-------
 Src/cyphal_application/lights/lights.hpp | 17 ++++++-
 2 files changed, 62 insertions(+), 19 deletions(-)

diff --git a/Src/cyphal_application/lights/lights.cpp b/Src/cyphal_application/lights/lights.cpp
index 3c59be7..be1d992 100644
--- a/Src/cyphal_application/lights/lights.cpp
+++ b/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();
 }
diff --git a/Src/cyphal_application/lights/lights.hpp b/Src/cyphal_application/lights/lights.hpp
index 7155fc8..1a491bb 100644
--- a/Src/cyphal_application/lights/lights.hpp
+++ b/Src/cyphal_application/lights/lights.hpp
@@ -8,6 +8,7 @@
 #include "cyphal_subscribers.hpp"
 #include "Udral/rgbled.hpp"
 #include "Udral/circuit_status.hpp"
+#include "periphery/ws2812/ws2812.h"
 
 class RgbLights {
 public:
@@ -15,9 +16,23 @@ class RgbLights {
     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_