From 6543ac4d954872b49ae372d0bf77c1268ff50b05 Mon Sep 17 00:00:00 2001
From: "Jasper v. Blanckenburg" <j.blanckenburg@fasttube.de>
Date: Mon, 22 May 2023 16:28:25 +0200
Subject: [PATCH] Use LEDs to show speed

---
 Core/Src/leds.c | 58 ++++++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 53 insertions(+), 5 deletions(-)

diff --git a/Core/Src/leds.c b/Core/Src/leds.c
index b703ea4..2117c35 100644
--- a/Core/Src/leds.c
+++ b/Core/Src/leds.c
@@ -3,11 +3,19 @@
 #include "app_azure_rtos.h"
 #include "main.h"
 
+#include "stm32h7xx_hal.h"
 #include "stm32h7xx_hal_dma.h"
 #include "stm32h7xx_hal_gpio.h"
 #include "stm32h7xx_hal_spi.h"
 #include "stm32h7xx_hal_tim.h"
 #include "tx_api.h"
+#include "vehicle.h"
+#include <stdint.h>
+
+#define LED_SPEED_MIN 15         // km/h
+#define LED_SPEED_MAX 80         // km/h
+#define LED_SPEED_HUE_MIN 180.0f // °
+#define LED_SPEED_HUE_MAX 0.0f   // °
 
 SPI_HandleTypeDef *hspi;
 TIM_HandleTypeDef *htim;
@@ -66,9 +74,37 @@ void led_thread_entry(ULONG child_thread_stack_addr) {
   led_start_animation(ANIM_RAINBOW);
   while (1) {
     tx_thread_sleep(10);
-    if (child_thread.tx_thread_state == TX_COMPLETED) {
-      tx_thread_sleep(20);
-      led_start_animation(ANIM_TE_STARTUP);
+    if (child_thread.tx_thread_stack_start &&
+        child_thread.tx_thread_state != TX_COMPLETED &&
+        child_thread.tx_thread_state != TX_TERMINATED) {
+      continue;
+    }
+
+    float speed =
+        (vehicle_state.speed - LED_SPEED_MIN) / (LED_SPEED_MAX - LED_SPEED_MIN);
+    float num_leds_exact = speed * N_LEDS;
+    num_leds_exact = fmin(N_LEDS, fmax(0, num_leds_exact));
+    int num_leds = num_leds_exact;
+    float num_leds_frac = num_leds_exact - num_leds;
+
+    float hue =
+        LED_SPEED_HUE_MIN - speed * (LED_SPEED_HUE_MIN - LED_SPEED_HUE_MAX);
+    hue = fmin(LED_SPEED_HUE_MIN, fmax(LED_SPEED_HUE_MAX, hue));
+    uint8_t r, g, b;
+    led_hsv_to_rgb(hue, 1.0f, 1.0f, &r, &g, &b);
+
+    // Fully illuminate first n LEDs
+    for (int i = 0; i < num_leds; i++) {
+      led_set(i, r, g, b);
+    }
+    // Partially illuminate n+1th LED
+    if (num_leds < N_LEDS) {
+      led_hsv_to_rgb(hue, 1.0f, num_leds_frac, &r, &g, &b);
+      led_set(num_leds, r, g, b);
+    }
+    // Turn off all other LEDs
+    for (int i = num_leds + 1; i < N_LEDS; i++) {
+      led_set(i, 0, 0, 0);
     }
   }
 }
@@ -160,7 +196,9 @@ void led_anim_knight_rider(ULONG _) {
 void led_anim_rainbow(ULONG _) {
   size_t tail = 0;
   float offset = 0.0f;
-  while (1) {
+  // Moving rainbow
+  uint32_t start = HAL_GetTick();
+  while (offset < 360) {
     for (size_t i = 0; i < N_LEDS; i++) {
       size_t led = (tail + i) % N_LEDS;
       uint8_t r, g, b;
@@ -168,7 +206,17 @@ void led_anim_rainbow(ULONG _) {
       led_hsv_to_rgb(hue, 1, 1, &r, &g, &b);
       led_set(led, r, g, b);
     }
-    offset = fmodf(offset + 2.0f, 360.0f);
+    offset = (HAL_GetTick() - start) / 5.0f;
+    tx_thread_sleep(1);
+  }
+  // Fade-out
+  for (float val = 1.0f; val > 0; val -= 0.1f) {
+    for (size_t i = 0; i < N_LEDS; i++) {
+      float hue = fmodf(offset + i * 360.0f / N_LEDS, 360.0f);
+      uint8_t r, g, b;
+      led_hsv_to_rgb(hue, 1, val, &r, &g, &b);
+      led_set(i, r, g, b);
+    }
     tx_thread_sleep(1);
   }
 }