Plausibility delay for SDC measurement

Core/Src/main.c

@@ -88,6 +88,16 @@ static void loop_delay() {
   }
   last_loop = HAL_GetTick();
 }
+
+static void update_sdc() {
+  static int last[2] = {0, 0};
+  int current = HAL_GPIO_ReadPin(SDC_VOLTAGE_GPIO_Port, SDC_VOLTAGE_Pin) == GPIO_PIN_SET;
+  if (last[0] == last[1] && last[0] == current) {
+    sdc_closed = current;
+  }
+  last[0] = last[1];
+  last[1] = current;
+}
 /* USER CODE END 0 */
 
 /**
@@ -138,8 +148,7 @@ int main(void)
     /* USER CODE END WHILE */
 
     /* USER CODE BEGIN 3 */
-    sdc_closed = HAL_GPIO_ReadPin(SDC_VOLTAGE_GPIO_Port, SDC_VOLTAGE_Pin) ==
+    update_sdc();
-                 GPIO_PIN_SET;
 
     slaves_check();
     shunt_check();

Core/Src/ts_state_machine.c

@@ -101,6 +101,7 @@ TSState ts_sm_update_precharge() {
 
 TSState ts_sm_update_discharge() {
   if (HAL_GetTick() - discharge_begin_timestamp >= DISCHARGE_DURATION) {
+    ts_state.target_state = TS_INACTIVE;
     return TS_INACTIVE;
   } else {
     return TS_DISCHARGE;