From 6c27b83377279a054983cbdbf35993018f8acd35 Mon Sep 17 00:00:00 2001
From: "Jasper v. Blanckenburg" <j.blanckenburg@fasttube.de>
Date: Mon, 24 Apr 2023 19:20:21 +0200
Subject: [PATCH] Motors turning

---
 Core/Inc/shunt_monitoring.h |  4 ++--
 Core/Inc/slave_monitoring.h |  2 +-
 Core/Src/shunt_monitoring.c |  8 ++++----
 Core/Src/ts_state_machine.c | 19 ++++++++++++-------
 4 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/Core/Inc/shunt_monitoring.h b/Core/Inc/shunt_monitoring.h
index 0a5aa6b..9440c65 100644
--- a/Core/Inc/shunt_monitoring.h
+++ b/Core/Inc/shunt_monitoring.h
@@ -9,8 +9,8 @@
 
 typedef struct {
   int32_t current;
-  int32_t voltage1;
-  int32_t voltage2;
+  int32_t voltage_bat;
+  int32_t voltage_veh;
   int32_t voltage3;
   int32_t busbartemp;
   int32_t power;
diff --git a/Core/Inc/slave_monitoring.h b/Core/Inc/slave_monitoring.h
index 6a807e1..7e63764 100644
--- a/Core/Inc/slave_monitoring.h
+++ b/Core/Inc/slave_monitoring.h
@@ -3,7 +3,7 @@
 
 #include <stdint.h>
 
-#define N_SLAVES 6
+#define N_SLAVES 5
 #define N_CELLS_SERIES 17
 #define N_CELLS_PARALLEL 5
 #define N_TEMP_SENSORS 32
diff --git a/Core/Src/shunt_monitoring.c b/Core/Src/shunt_monitoring.c
index 8ecf063..3b601ff 100644
--- a/Core/Src/shunt_monitoring.c
+++ b/Core/Src/shunt_monitoring.c
@@ -12,8 +12,8 @@ ShuntData shunt_data;
 
 void shunt_init() {
   shunt_data.current = 0;
-  shunt_data.voltage1 = 0;
-  shunt_data.voltage2 = 0;
+  shunt_data.voltage_veh = 0;
+  shunt_data.voltage_bat = 0;
   shunt_data.voltage3 = 0;
   shunt_data.busbartemp = 0;
   shunt_data.power = 0;
@@ -36,10 +36,10 @@ void shunt_handle_can_msg(uint16_t id, const uint8_t *data) {
     shunt_data.current = result;
     break;
   case CAN_ID_SHUNT_VOLTAGE1:
-    shunt_data.voltage1 = result;
+    shunt_data.voltage_bat = result;
     break;
   case CAN_ID_SHUNT_VOLTAGE2:
-    shunt_data.voltage2 = result;
+    shunt_data.voltage_veh = result;
     break;
   case CAN_ID_SHUNT_VOLTAGE3:
     shunt_data.voltage3 = result;
diff --git a/Core/Src/ts_state_machine.c b/Core/Src/ts_state_machine.c
index c3fc778..9d3c993 100644
--- a/Core/Src/ts_state_machine.c
+++ b/Core/Src/ts_state_machine.c
@@ -12,6 +12,11 @@ static uint32_t precharge_95_reached_timestamp = 0;
 static uint32_t charging_check_timestamp = 0;
 static uint32_t discharge_begin_timestamp = 0;
 
+static int sdc_is_closed() {
+  return HAL_GPIO_ReadPin(SDC_VOLTAGE_GPIO_Port, SDC_VOLTAGE_Pin) ==
+         GPIO_PIN_SET;
+}
+
 void ts_sm_init() {
   ts_state.current_state = TS_INACTIVE;
   ts_state.target_state = TS_INACTIVE;
@@ -63,7 +68,7 @@ TSState ts_sm_update_inactive() {
 }
 
 TSState ts_sm_update_active() {
-  if (ts_state.target_state == TS_INACTIVE) {
+  if (ts_state.target_state == TS_INACTIVE || !sdc_is_closed()) {
     discharge_begin_timestamp = HAL_GetTick();
     return TS_DISCHARGE;
   }
@@ -72,12 +77,12 @@ TSState ts_sm_update_active() {
 }
 
 TSState ts_sm_update_precharge() {
-  if (ts_state.target_state == TS_INACTIVE) {
+  if (ts_state.target_state == TS_INACTIVE || !sdc_is_closed()) {
     discharge_begin_timestamp = HAL_GetTick();
     return TS_DISCHARGE;
   }
-  if (shunt_data.voltage2 > MIN_VEHICLE_SIDE_VOLTAGE &&
-      shunt_data.voltage2 > 0.95 * shunt_data.voltage3) {
+  if (shunt_data.voltage_veh > MIN_VEHICLE_SIDE_VOLTAGE &&
+      shunt_data.voltage_veh > 0.95 * shunt_data.voltage_bat) {
     uint32_t now = HAL_GetTick();
     if (precharge_95_reached_timestamp == 0) {
       precharge_95_reached_timestamp = now;
@@ -114,12 +119,12 @@ TSState ts_sm_update_error() {
 }
 
 TSState ts_sm_update_charging_check() {
-  if (ts_state.target_state == TS_INACTIVE) {
+  if (ts_state.target_state == TS_INACTIVE || !sdc_is_closed()) {
     discharge_begin_timestamp = HAL_GetTick();
     return TS_DISCHARGE;
   }
 
-  if (shunt_data.voltage2 > shunt_data.voltage3) {
+  if (shunt_data.voltage_veh > shunt_data.voltage_bat) {
     return TS_CHARGING;
   } else if (HAL_GetTick() - charging_check_timestamp >
              MAX_CHARGING_CHECK_DURATION) {
@@ -130,7 +135,7 @@ TSState ts_sm_update_charging_check() {
 }
 
 TSState ts_sm_update_charging() {
-  if (ts_state.target_state == TS_INACTIVE) {
+  if (ts_state.target_state == TS_INACTIVE || !sdc_is_closed()) {
     discharge_begin_timestamp = HAL_GetTick();
     return TS_DISCHARGE;
   }