From aee7f4f697498287e64ee5e23bae0dc52ba38965 Mon Sep 17 00:00:00 2001
From: Leonard Gies <l.gies@fasttube.de>
Date: Sat, 12 Jul 2025 18:15:44 +0200
Subject: [PATCH] =?UTF-8?q?nicht=20wirklich=20sch=C3=B6nere=20Fixes=20f?=
 =?UTF-8?q?=C3=BCr=20FSN?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Software/Core/Src/can.c     |  6 +++---
 Software/Core/Src/main.c    |  6 +++++-
 Software/Core/Src/scs_sdc.c | 23 +++++++++++++----------
 3 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/Software/Core/Src/can.c b/Software/Core/Src/can.c
index 9dd9677..abeba7e 100644
--- a/Software/Core/Src/can.c
+++ b/Software/Core/Src/can.c
@@ -6,7 +6,6 @@
 #include "can-halal.h"
 
 extern int can_ams_last_tick;
-extern TSState request_state;
 
 HAL_StatusTypeDef can_init(FDCAN_HandleTypeDef* handle) {
   TRY(ftcan_init(handle));
@@ -18,9 +17,9 @@ HAL_StatusTypeDef can_init(FDCAN_HandleTypeDef* handle) {
 
 HAL_StatusTypeDef can_update_state(){
   uint8_t data;
-  if (request_state == TS_ACTIVE){
+  if (ts_state.target_state == TS_ACTIVE){
     data = 0x01;
-  } else if (request_state == TS_INACTIVE) {
+  } else if (ts_state.target_state == TS_INACTIVE) {
     data = 0x00;
   } else {
     return HAL_ERROR;
@@ -38,6 +37,7 @@ void ftcan_msg_received_cb(uint16_t id, size_t len, const uint8_t* data) {
     case CAN_ID_AMS_SIGNALS:
       break;
     case CAN_ID_AMS_ERROR:
+      ts_state.target_state = TS_INACTIVE;
       break;
     default:
       break;
diff --git a/Software/Core/Src/main.c b/Software/Core/Src/main.c
index 3212d9c..c98411c 100644
--- a/Software/Core/Src/main.c
+++ b/Software/Core/Src/main.c
@@ -149,6 +149,7 @@ int main(void)
     /* USER CODE BEGIN 3 */
     if (HAL_GetTick() - can_ams_last_tick > AMS_TIMEOUT) {
       error_led_init();
+      change_ts_state(TS_INACTIVE);
       led_blink(&htim3, COLOR_ERROR, 0);
     } else {
       led_blink(&htim3, COLOR_HEARTBEAT, 1000);
@@ -157,11 +158,14 @@ int main(void)
     // SHOW_STATE(check_sdc_state());
     // SHOW_STATE(can_update_state());
 	  TSON = HAL_GPIO_ReadPin(TS_ON_GPIO_Port, TS_ON_Pin);
-    if (TSON == 1){
+    uint8_t sdc_closed = HAL_GPIO_ReadPin(SDC_ON_GPIO_Port, SDC_ON_Pin);
+    if (TSON == 1 && sdc_closed) {
       change_ts_state(TS_ACTIVE);
     }
     check_sdc_state();
     can_update_state();
+
+    HAL_Delay(100);
   }
   /* USER CODE END 3 */
 }
diff --git a/Software/Core/Src/scs_sdc.c b/Software/Core/Src/scs_sdc.c
index fdb4a60..9ee5154 100644
--- a/Software/Core/Src/scs_sdc.c
+++ b/Software/Core/Src/scs_sdc.c
@@ -2,10 +2,9 @@
 #include "can.h"
 #include "scs_sdc.h"
 
-TSStateHandle ts_state;
+TSStateHandle ts_state = {.current_state = TS_INACTIVE, .target_state = TS_INACTIVE};
 IMDData imd_data;
 uint8_t led_state;
-TSState request_state = TS_INACTIVE;
 
 void error_led_init() {
   HAL_GPIO_WritePin(AMS_ERROR_GPIO_Port, AMS_ERROR_Pin, GPIO_PIN_SET);
@@ -21,26 +20,30 @@ void ts_handle_ams_status(const uint8_t* data) {
   imd_data.ok = data[6] & 0b10000000;
   led_state = data[7];
 
-  if ((led_state | 0b00000010) == 0b10) {
+  if ((led_state & 0b00000010) == 0b10) {
+      HAL_GPIO_WritePin(IMD_ERROR_GPIO_Port, IMD_ERROR_Pin, GPIO_PIN_SET);
+  } else {
+      HAL_GPIO_WritePin(IMD_ERROR_GPIO_Port, IMD_ERROR_Pin, GPIO_PIN_RESET);
+  }
+
+  if ((led_state & 0b00000100) == 0b100) {
       HAL_GPIO_WritePin(AMS_ERROR_GPIO_Port, AMS_ERROR_Pin, GPIO_PIN_SET);
   } else {
       HAL_GPIO_WritePin(AMS_ERROR_GPIO_Port, AMS_ERROR_Pin, GPIO_PIN_RESET);
   }
 
-  if ((led_state | 0b00000100) == 0b100) {
-      HAL_GPIO_WritePin(IMD_ERROR_GPIO_Port, IMD_ERROR_Pin, GPIO_PIN_SET);
-  } else {
-      HAL_GPIO_WritePin(IMD_ERROR_GPIO_Port, IMD_ERROR_Pin, GPIO_PIN_RESET);
+  if (ts_state.current_state == TS_ERROR || ts_state.current_state == TS_DISCHARGE || ((led_state & 0b110) > 0)) {
+    ts_state.target_state = TS_INACTIVE;
   }
 }
 
 HAL_StatusTypeDef change_ts_state(TSState state) {
   if (state == TS_ACTIVE) {
-    request_state = TS_ACTIVE;
-    
+    ts_state.target_state = TS_ACTIVE;
+
     return HAL_OK;
   } else if (state == TS_INACTIVE){
-    request_state = TS_INACTIVE;
+    ts_state.target_state = TS_INACTIVE;
 
     return HAL_OK;
   } else {