From 4354b039070c28acfe4a1375c9441ddc17532894 Mon Sep 17 00:00:00 2001
From: Leonard Gies <l.gies@fasttube.de>
Date: Tue, 22 Jul 2025 18:12:43 +0200
Subject: [PATCH] add uncalibrated version of lv soc

---
 Core/Inc/vehicle_state.h                      |  1 +
 Core/Src/vehicle.c                            | 44 +++++++++++++++++++
 .../gui/include/gui/common/NamedField.hpp     |  1 +
 TouchGFX/gui/src/common/NamedField.cpp        |  1 +
 4 files changed, 47 insertions(+)

diff --git a/Core/Inc/vehicle_state.h b/Core/Inc/vehicle_state.h
index 104d76e..b22a5f3 100644
--- a/Core/Inc/vehicle_state.h
+++ b/Core/Inc/vehicle_state.h
@@ -165,6 +165,7 @@ typedef struct {
   float tank_pressure_2;
 
   float lv_voltage;
+  float lv_soc;
 
   uint8_t apps_percent;
   float brake_pressure_f;
diff --git a/Core/Src/vehicle.c b/Core/Src/vehicle.c
index dd59e88..5805520 100644
--- a/Core/Src/vehicle.c
+++ b/Core/Src/vehicle.c
@@ -64,6 +64,24 @@
 #define CAN_ID_STW_BUTTONS 0x401
 #define CAN_ID_STW_PARAM_SET 0x402
 
+#define NUM_LV_SOC_VOLTAGE_MAP 11
+struct { // percent and voltage must be monotonically decreasing
+  uint8_t percent;
+  float voltage;
+} LV_SOC_VOLTAGE_MAP[NUM_LV_SOC_VOLTAGE_MAP] = {
+    {.percent = 100, .voltage = 27.20}, //
+    {.percent = 90, .voltage = 26.80},  //
+    {.percent = 80, .voltage = 26.56},  //
+    {.percent = 70, .voltage = 26.40},  //
+    {.percent = 60, .voltage = 26.16},  //
+    {.percent = 50, .voltage = 26.08},  //
+    {.percent = 40, .voltage = 26.00},  //
+    {.percent = 30, .voltage = 25.76},  //
+    {.percent = 20, .voltage = 25.60},  //
+    {.percent = 10, .voltage = 24.00},  //
+    {.percent = 0, .voltage = 20.00},   //
+};
+
 void vehicle_thread_entry(ULONG hfdcan_addr) {
   memset(&vehicle_state, 0, sizeof(vehicle_state));
 
@@ -140,6 +158,31 @@ void vehicle_broadcast_buttons(GPIO_PinState *button_states) {
   ftcan_transmit(CAN_ID_STW_BUTTONS, &data, 1);
 }
 
+float calculate_lv_soc(float lv_voltage) {
+  // check if lv_voltage is in range of voltage map
+  if (lv_voltage >= LV_SOC_VOLTAGE_MAP[0].voltage) {
+    return LV_SOC_VOLTAGE_MAP[0].percent;
+  }
+  if (lv_voltage <= LV_SOC_VOLTAGE_MAP[NUM_LV_SOC_VOLTAGE_MAP - 1].voltage) {
+    return LV_SOC_VOLTAGE_MAP[NUM_LV_SOC_VOLTAGE_MAP - 1].percent;
+  }
+
+  size_t index_upper_volt = 0;
+  size_t index_lower_volt = 1;
+
+  while (index_lower_volt < NUM_LV_SOC_VOLTAGE_MAP && !(lv_voltage <= LV_SOC_VOLTAGE_MAP[index_upper_volt].voltage &&
+                                                        lv_voltage >= LV_SOC_VOLTAGE_MAP[index_lower_volt].voltage)) {
+    index_upper_volt++;
+    index_lower_volt++;
+  }
+
+  // calculate f value between 0 and 1 for linear interpolation
+  float f = (lv_voltage - LV_SOC_VOLTAGE_MAP[index_lower_volt].voltage) /
+            (LV_SOC_VOLTAGE_MAP[index_upper_volt].voltage - LV_SOC_VOLTAGE_MAP[index_lower_volt].voltage);
+
+  return LV_SOC_VOLTAGE_MAP[index_lower_volt].percent * (1.0 - f) + LV_SOC_VOLTAGE_MAP[index_upper_volt].percent * f;
+}
+
 void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
   const uint8_t *ptr;
   switch (id) {
@@ -266,6 +309,7 @@ void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
     case CAN_ID_PDU_CURRENT4:
       ptr = &data[2];
       vehicle_state.lv_voltage = ftcan_unmarshal_signed(&ptr, 2) * 0.001f;
+      vehicle_state.lv_soc = calculate_lv_soc(vehicle_state.lv_voltage);
       break;
     case CAN_ID_DASHBOARD:
       // TODO
diff --git a/TouchGFX/gui/include/gui/common/NamedField.hpp b/TouchGFX/gui/include/gui/common/NamedField.hpp
index 9cf851b..7fcb0f2 100644
--- a/TouchGFX/gui/include/gui/common/NamedField.hpp
+++ b/TouchGFX/gui/include/gui/common/NamedField.hpp
@@ -55,6 +55,7 @@ CountedEnum(DataFieldType, size_t,
   DF_TankPressure1,
   DF_TankPressure2,
   DF_LVVoltage,
+  DF_LVSOC,
   DF_APPSPercent,
   DF_BPF,
   DF_BPR,
diff --git a/TouchGFX/gui/src/common/NamedField.cpp b/TouchGFX/gui/src/common/NamedField.cpp
index e766a9d..0bc6d81 100644
--- a/TouchGFX/gui/src/common/NamedField.cpp
+++ b/TouchGFX/gui/src/common/NamedField.cpp
@@ -307,6 +307,7 @@ NamedFieldDescription dataFieldDescs[] = {
     [DF_TankPressure1] = {NamedFieldKind::Float, "TnkPrs1", 3, 1, VEH_FIELD(tank_pressure_1)},
     [DF_TankPressure2] = {NamedFieldKind::Float, "TnkPrs2", 3, 1, VEH_FIELD(tank_pressure_2)},
     [DF_LVVoltage] = {NamedFieldKind::Float, "LV_V", 2, 2, VEH_FIELD(lv_voltage)},
+    [DF_LVSOC] = {NamedFieldKind::Float, "LV_SOC", 2, 0, VEH_FIELD(lv_soc)},
     [DF_APPSPercent] = {NamedFieldKind::Int, "APPS", 3, 0, VEH_FIELD(apps_percent)},
     [DF_BPF] = {NamedFieldKind::Float, "BPF", 3, 1, VEH_FIELD(brake_pressure_f)},
     [DF_BPR] = {NamedFieldKind::Float, "BPR", 3, 1, VEH_FIELD(brake_pressure_r)},