Add regen overcurrent limit

The current counter on the shunt can't be activated for some reason.

Core/Inc/can.h

@@ -5,10 +5,14 @@
 #include "stm32f3xx_hal_can.h"
 #include "stm32f3xx_hal_def.h"
 
+#include "ts_state_machine.h"
+
 #define CAN_ID_SLAVE_PANIC 0x009
 #define CAN_ID_AMS_STATUS 0x00A
 #define CAN_ID_AMS_IN 0x00B
-#define CAN_ID_SLAVE_STATUS 0x014
+#define CAN_ID_AMS_ERROR 0x00C
+#define CAN_ID_SLAVE_STATUS_BASE 0x080
+#define CAN_ID_SLAVE_STATUS_FUCKED 0x0A0
 #define CAN_ID_SLAVE_LOG 0x4F4
 #define CAN_ID_SHUNT_BASE 0x520
 #define CAN_ID_SHUNT_CURRENT 0x521
@@ -22,6 +26,7 @@
 
 void can_init(CAN_HandleTypeDef *handle);
 HAL_StatusTypeDef can_send_status();
+HAL_StatusTypeDef can_send_error(TSErrorKind kind, uint8_t arg);
 
 void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data);
 

Core/Inc/imd_monitoring.h

@@ -0,0 +1,36 @@
+#ifndef INC_IMD_MONITORING_H
+#define INC_IMD_MONITORING_H
+
+#include <stdint.h>
+
+#include "stm32f3xx_hal.h"
+
+typedef enum {
+  IMD_STATE_UNKNOWN,
+  IMD_STATE_SHORTCIRCUIT_SUPPLY,
+  IMD_STATE_SHORTCIRCUIT_GND,
+  IMD_STATE_NORMAL,
+  IMD_STATE_UNDERVOLTAGE,
+  IMD_STATE_SST,
+  IMD_STATE_DEV_ERROR,
+  IMD_STATE_GND_FAULT,
+} IMDState;
+
+typedef struct {
+  int ok;
+
+  IMDState state;
+  uint32_t r_iso;
+
+  uint32_t freq;
+  uint32_t duty_cycle;
+  uint32_t last_high;
+} IMDData;
+
+extern IMDData imd_data;
+
+void imd_init(TIM_HandleTypeDef *htim);
+
+void imd_update(void);
+
+#endif // INC_IMD_MONITORING_H

Core/Inc/main.h

@@ -41,7 +41,7 @@ extern "C" {
 
 /* Exported constants --------------------------------------------------------*/
 /* USER CODE BEGIN EC */
-
+extern int sdc_closed;
 /* USER CODE END EC */
 
 /* Exported macro ------------------------------------------------------------*/
@@ -61,6 +61,10 @@ void Error_Handler(void);
 #define HV_MISMATCH_ERR_GPIO_Port GPIOA
 #define RELAY_MISMATCH_ERR_Pin GPIO_PIN_1
 #define RELAY_MISMATCH_ERR_GPIO_Port GPIOA
+#define IMD_M_Pin GPIO_PIN_2
+#define IMD_M_GPIO_Port GPIOA
+#define IMD_OK_Pin GPIO_PIN_3
+#define IMD_OK_GPIO_Port GPIOA
 #define RELAY_CONNECTION_ERR_Pin GPIO_PIN_4
 #define RELAY_CONNECTION_ERR_GPIO_Port GPIOA
 #define HV_ACTIVE_Pin GPIO_PIN_5

Core/Inc/shunt_monitoring.h

@@ -5,19 +5,23 @@
 
 #include "stm32f3xx_hal.h"
 
-#define THRESH_OVERCURRENT 300000 // mA
+#define SHUNT_TIMEOUT 300                    // ms
+#define SHUNT_THRESH_OVERCURRENT 300000      // mA
+#define SHUNT_THRESH_OVERCURRENT_REGEN 50000 // mA
+#define SHUNT_THRESH_OVERTEMP 1000           // 1/10 °C
 
 typedef struct {
-  int32_t current;
+  int32_t current;     // mA
-  int32_t voltage1;
+  int32_t voltage_bat; // mV
-  int32_t voltage2;
+  int32_t voltage_veh; // mV
-  int32_t voltage3;
+  int32_t voltage3;    // mV
   int32_t busbartemp;
   int32_t power;
   int32_t energy;
-  int32_t current_counter;
+  float current_counter; // mAs
 
   uint32_t last_message;
+  uint32_t last_current_message;
 } ShuntData;
 extern ShuntData shunt_data;
 

Core/Inc/soc_estimation.h

@@ -3,9 +3,16 @@
 
 #include <stdint.h>
 
-extern uint8_t current_soc;
+extern float current_soc;
 
 void soc_init();
 void soc_update();
 
+typedef struct {
+  uint16_t ocv;
+  float soc;
+} ocv_soc_pair_t;
+extern ocv_soc_pair_t OCV_SOC_PAIRS[];
+float soc_for_ocv(uint16_t ocv);
+
 #endif // INC_SOC_ESTIMATION_H

Core/Inc/stm32f3xx_hal_conf.h

@@ -57,7 +57,7 @@
 /*#define HAL_RNG_MODULE_ENABLED   */
 /*#define HAL_RTC_MODULE_ENABLED   */
 /*#define HAL_SPI_MODULE_ENABLED   */
-/*#define HAL_TIM_MODULE_ENABLED   */
+#define HAL_TIM_MODULE_ENABLED
 #define HAL_UART_MODULE_ENABLED
 /*#define HAL_USART_MODULE_ENABLED   */
 /*#define HAL_IRDA_MODULE_ENABLED   */

Core/Inc/stm32f3xx_it.h

@@ -56,6 +56,7 @@ void DebugMon_Handler(void);
 void PendSV_Handler(void);
 void SysTick_Handler(void);
 void USB_LP_CAN_RX0_IRQHandler(void);
+void TIM1_BRK_TIM15_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 
 /* USER CODE END EFP */

Core/Inc/ts_state_machine.h

@@ -8,7 +8,8 @@
 // Minimum vehicle side voltage to exit precharge
 #define MIN_VEHICLE_SIDE_VOLTAGE 150000 // mV
 // Time to wait after reaching 95% of battery voltage before exiting precharge
-#define PRECHARGE_95_DURATION 500 // ms
+// Set this to 1000 in scruti to demonstrate the voltage on the multimeter
+#define PRECHARGE_95_DURATION 0 // ms
 // Time to wait for discharge
 #define DISCHARGE_DURATION 5000 // ms
 // Time to wait after there is no more error condition before exiting TS_ERROR
@@ -28,6 +29,14 @@ typedef enum {
   TS_CHARGING
 } TSState;
 
+typedef enum {
+  TS_ERRORKIND_NONE = 0x00,
+  TS_ERRORKIND_SLAVE_TIMEOUT = 0x01,
+  TS_ERRORKIND_SLAVE_PANIC = 0x02,
+  TS_ERRORKIND_SHUNT_TIMEOUT = 0x03,
+  TS_ERRORKIND_SHUNT_OVERCURRENT = 0x04,
+  TS_ERRORKIND_SHUNT_OVERTEMP = 0x05
+} TSErrorKind;
 #define TS_ERROR_SOURCE_SHUNT (1 << 0)
 #define TS_ERROR_SOURCE_SLAVES (1 << 1)
 

Core/Src/can.c

@@ -1,5 +1,6 @@
 #include "can.h"
 
+#include "imd_monitoring.h"
 #include "main.h"
 #include "shunt_monitoring.h"
 #include "slave_monitoring.h"
@@ -8,6 +9,7 @@
 
 #include "can-halal.h"
 
+#include <math.h>
 #include <stdint.h>
 
 void can_init(CAN_HandleTypeDef *handle) {
@@ -15,31 +17,47 @@ void can_init(CAN_HandleTypeDef *handle) {
   ftcan_add_filter(CAN_ID_SHUNT_BASE, 0xFF0);
   ftcan_add_filter(CAN_ID_AMS_IN, 0xFFF);
   ftcan_add_filter(CAN_ID_SLAVE_PANIC, 0xFFF);
-  ftcan_add_filter(CAN_ID_SLAVE_STATUS, 0xFFF);
+  ftcan_add_filter(CAN_ID_SLAVE_STATUS_BASE, 0xFF0);
+  ftcan_add_filter(CAN_ID_SLAVE_STATUS_FUCKED, 0xFFF);
   ftcan_add_filter(CAN_ID_SLAVE_LOG, 0xFFF);
 }
 
 HAL_StatusTypeDef can_send_status() {
-  uint8_t data[6];
+  uint8_t data[8];
-  data[0] = ts_state.current_state;
+  data[0] = ts_state.current_state | (sdc_closed << 7);
-  data[1] = current_soc;
+  data[1] = roundf(current_soc);
   ftcan_marshal_unsigned(&data[2], min_voltage, 2);
   ftcan_marshal_signed(&data[4], max_temp, 2);
+  data[6] = imd_data.state | (imd_data.ok << 7);
+  if (imd_data.r_iso < 0xFFF) {
+    data[7] = imd_data.r_iso >> 4;
+  } else {
+    data[7] = 0xFF;
+  }
   return ftcan_transmit(CAN_ID_AMS_STATUS, data, sizeof(data));
 }
 
+HAL_StatusTypeDef can_send_error(TSErrorKind kind, uint8_t arg) {
+  uint8_t data[2];
+  data[0] = kind;
+  data[1] = arg;
+  return ftcan_transmit(CAN_ID_AMS_ERROR, data, sizeof(data));
+}
+
 void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
   if ((id & 0xFF0) == CAN_ID_SHUNT_BASE) {
     shunt_handle_can_msg(id, data);
     return;
+  } else if ((id & 0xFF0) == CAN_ID_SLAVE_STATUS_BASE) {
+    slaves_handle_status(data);
+    return;
+  } else if (id == CAN_ID_SLAVE_STATUS_FUCKED) {
+    slaves_handle_status(data);
   }
   switch (id) {
   case CAN_ID_SLAVE_PANIC:
     slaves_handle_panic(data);
     break;
-  case CAN_ID_SLAVE_STATUS:
-    slaves_handle_status(data);
-    break;
   case CAN_ID_SLAVE_LOG:
     slaves_handle_log(data);
     break;

Core/Src/imd_monitoring.c

@@ -0,0 +1,86 @@
+#include "imd_monitoring.h"
+
+#include "main.h"
+
+#define FREQ_TIMER 1000 // Hz
+
+#define FREQ_TOLERANCE 1     // Hz
+#define FREQ_NORMAL 10       // Hz
+#define FREQ_UNDERVOLTAGE 20 // Hz
+#define FREQ_SST 30          // Hz
+#define FREQ_DEV_ERROR 40    // Hz
+#define FREQ_GND_FAULT 50    // Hz
+
+#define RISO_MIN_DUTY_CYCLE 8 // %
+#define RISO_MAX 50000        // kOhm
+
+#define PWM_TIMEOUT 200 // ms
+
+IMDData imd_data;
+
+static TIM_HandleTypeDef *htim;
+
+void imd_init(TIM_HandleTypeDef *handle) {
+  htim = handle;
+  HAL_TIM_IC_Start_IT(htim, TIM_CHANNEL_1);
+  HAL_TIM_IC_Start(htim, TIM_CHANNEL_2);
+
+  imd_data.state = IMD_STATE_UNKNOWN;
+}
+
+void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *handle) {
+  if (handle != htim || htim->Channel != HAL_TIM_ACTIVE_CHANNEL_1) {
+    return;
+  }
+  uint32_t period = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
+  if (period == 0) {
+    // First edge, ignore
+    return;
+  }
+  imd_data.last_high = HAL_GetTick();
+
+  imd_data.freq = FREQ_TIMER / period;
+  uint32_t high_time = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
+  imd_data.duty_cycle = (100 * high_time) / period;
+
+  // Check PWM frequency for state determination
+  if (imd_data.freq > FREQ_NORMAL - FREQ_TOLERANCE &&
+      imd_data.freq < FREQ_NORMAL + FREQ_TOLERANCE) {
+    imd_data.state = IMD_STATE_NORMAL;
+  } else if (imd_data.freq > FREQ_UNDERVOLTAGE - FREQ_TOLERANCE &&
+             imd_data.freq < FREQ_UNDERVOLTAGE + FREQ_TOLERANCE) {
+    imd_data.state = IMD_STATE_UNDERVOLTAGE;
+  } else if (imd_data.freq > FREQ_SST - FREQ_TOLERANCE &&
+             imd_data.freq < FREQ_SST + FREQ_TOLERANCE) {
+    imd_data.state = IMD_STATE_SST;
+  } else if (imd_data.freq > FREQ_DEV_ERROR - FREQ_TOLERANCE &&
+             imd_data.freq < FREQ_DEV_ERROR + FREQ_TOLERANCE) {
+    imd_data.state = IMD_STATE_DEV_ERROR;
+  } else if (imd_data.freq > FREQ_GND_FAULT - FREQ_TOLERANCE &&
+             imd_data.freq < FREQ_GND_FAULT + FREQ_TOLERANCE) {
+    imd_data.state = IMD_STATE_GND_FAULT;
+  } else {
+    imd_data.state = IMD_STATE_UNKNOWN;
+  }
+
+  // Calculate R_iso
+  if (imd_data.state == IMD_STATE_NORMAL ||
+      imd_data.state == IMD_STATE_UNDERVOLTAGE) {
+    if (imd_data.duty_cycle < RISO_MIN_DUTY_CYCLE) {
+      imd_data.r_iso = RISO_MAX;
+    } else {
+      imd_data.r_iso = (90 * 1200) / (imd_data.duty_cycle - 5) - 1200;
+    }
+  }
+}
+
+void imd_update() {
+  imd_data.ok = HAL_GPIO_ReadPin(IMD_OK_GPIO_Port, IMD_OK_Pin);
+  if (HAL_GetTick() - imd_data.last_high > PWM_TIMEOUT) {
+    if (HAL_GPIO_ReadPin(IMD_M_GPIO_Port, IMD_M_Pin) == GPIO_PIN_SET) {
+      imd_data.state = IMD_STATE_SHORTCIRCUIT_SUPPLY;
+    } else {
+      imd_data.state = IMD_STATE_SHORTCIRCUIT_GND;
+    }
+  }
+}

Core/Src/main.c

@@ -22,14 +22,15 @@
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
 #include "can.h"
-#include "soc_estimation.h"
+#include "imd_monitoring.h"
-#include "stm32f3xx_hal.h"
-#include "stm32f3xx_hal_gpio.h"
-
 #include "shunt_monitoring.h"
 #include "slave_monitoring.h"
+#include "soc_estimation.h"
 #include "ts_state_machine.h"
 
+#include "stm32f3xx_hal.h"
+#include "stm32f3xx_hal_gpio.h"
+
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -52,10 +53,14 @@ ADC_HandleTypeDef hadc2;
 
 CAN_HandleTypeDef hcan;
 
+I2C_HandleTypeDef hi2c1;
+
+TIM_HandleTypeDef htim15;
+
 UART_HandleTypeDef huart1;
 
 /* USER CODE BEGIN PV */
-
+int sdc_closed = 0;
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
@@ -64,13 +69,27 @@ static void MX_GPIO_Init(void);
 static void MX_ADC2_Init(void);
 static void MX_CAN_Init(void);
 static void MX_USART1_UART_Init(void);
+static void MX_I2C1_Init(void);
+static void MX_TIM15_Init(void);
 /* USER CODE BEGIN PFP */
 
 /* USER CODE END PFP */
 
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
+#define MAIN_LOOP_PERIOD 50
 
+static void loop_delay() {
+  static uint32_t last_loop = 0;
+  uint32_t dt = HAL_GetTick() - last_loop;
+  if (dt < MAIN_LOOP_PERIOD) {
+    HAL_Delay(MAIN_LOOP_PERIOD - dt);
+    HAL_GPIO_WritePin(STATUS2_GPIO_Port, STATUS2_Pin, GPIO_PIN_RESET);
+  } else {
+    HAL_GPIO_WritePin(STATUS2_GPIO_Port, STATUS2_Pin, GPIO_PIN_SET);
+  }
+  last_loop = HAL_GetTick();
+}
 /* USER CODE END 0 */
 
 /**
@@ -104,12 +123,15 @@ int main(void) {
   MX_ADC2_Init();
   MX_CAN_Init();
   MX_USART1_UART_Init();
+  MX_I2C1_Init();
+  MX_TIM15_Init();
   /* USER CODE BEGIN 2 */
   can_init(&hcan);
   slaves_init();
   shunt_init();
   ts_sm_init();
   soc_init();
+  imd_init(&htim15);
   HAL_GPIO_WritePin(AMS_NERROR_GPIO_Port, AMS_NERROR_Pin, GPIO_PIN_SET);
   /* USER CODE END 2 */
 
@@ -120,14 +142,17 @@ int main(void) {
 
     /* USER CODE BEGIN 3 */
     HAL_GPIO_TogglePin(STATUS1_GPIO_Port, STATUS1_Pin);
+    sdc_closed = HAL_GPIO_ReadPin(SDC_VOLTAGE_GPIO_Port, SDC_VOLTAGE_Pin) ==
+                 GPIO_PIN_SET;
 
     slaves_check();
     shunt_check();
     ts_sm_update();
     soc_update();
+    imd_update();
     can_send_status();
 
-    HAL_Delay(10);
+    loop_delay();
   }
   /* USER CODE END 3 */
 }
@@ -169,9 +194,10 @@ void SystemClock_Config(void) {
     Error_Handler();
   }
   PeriphClkInit.PeriphClockSelection =
-      RCC_PERIPHCLK_USART1 | RCC_PERIPHCLK_ADC12;
+      RCC_PERIPHCLK_USART1 | RCC_PERIPHCLK_I2C1 | RCC_PERIPHCLK_ADC12;
   PeriphClkInit.Usart1ClockSelection = RCC_USART1CLKSOURCE_PCLK2;
   PeriphClkInit.Adc12ClockSelection = RCC_ADC12PLLCLK_DIV1;
+  PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_HSI;
   if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) {
     Error_Handler();
   }
@@ -264,6 +290,108 @@ static void MX_CAN_Init(void) {
   /* USER CODE END CAN_Init 2 */
 }
 
+/**
+ * @brief I2C1 Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_I2C1_Init(void) {
+
+  /* USER CODE BEGIN I2C1_Init 0 */
+
+  /* USER CODE END I2C1_Init 0 */
+
+  /* USER CODE BEGIN I2C1_Init 1 */
+
+  /* USER CODE END I2C1_Init 1 */
+  hi2c1.Instance = I2C1;
+  hi2c1.Init.Timing = 0x2000090E;
+  hi2c1.Init.OwnAddress1 = 0;
+  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+  hi2c1.Init.OwnAddress2 = 0;
+  hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
+  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+  if (HAL_I2C_Init(&hi2c1) != HAL_OK) {
+    Error_Handler();
+  }
+
+  /** Configure Analogue filter
+   */
+  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK) {
+    Error_Handler();
+  }
+
+  /** Configure Digital filter
+   */
+  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK) {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN I2C1_Init 2 */
+
+  /* USER CODE END I2C1_Init 2 */
+}
+
+/**
+ * @brief TIM15 Initialization Function
+ * @param None
+ * @retval None
+ */
+static void MX_TIM15_Init(void) {
+
+  /* USER CODE BEGIN TIM15_Init 0 */
+
+  /* USER CODE END TIM15_Init 0 */
+
+  TIM_SlaveConfigTypeDef sSlaveConfig = {0};
+  TIM_IC_InitTypeDef sConfigIC = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM15_Init 1 */
+
+  /* USER CODE END TIM15_Init 1 */
+  htim15.Instance = TIM15;
+  htim15.Init.Prescaler = 16000 - 1;
+  htim15.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim15.Init.Period = 65535;
+  htim15.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim15.Init.RepetitionCounter = 0;
+  htim15.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_IC_Init(&htim15) != HAL_OK) {
+    Error_Handler();
+  }
+  sSlaveConfig.SlaveMode = TIM_SLAVEMODE_RESET;
+  sSlaveConfig.InputTrigger = TIM_TS_TI1FP1;
+  sSlaveConfig.TriggerPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
+  sSlaveConfig.TriggerPrescaler = TIM_ICPSC_DIV1;
+  sSlaveConfig.TriggerFilter = 0;
+  if (HAL_TIM_SlaveConfigSynchro(&htim15, &sSlaveConfig) != HAL_OK) {
+    Error_Handler();
+  }
+  sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
+  sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
+  sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
+  sConfigIC.ICFilter = 0;
+  if (HAL_TIM_IC_ConfigChannel(&htim15, &sConfigIC, TIM_CHANNEL_1) != HAL_OK) {
+    Error_Handler();
+  }
+  sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_FALLING;
+  sConfigIC.ICSelection = TIM_ICSELECTION_INDIRECTTI;
+  if (HAL_TIM_IC_ConfigChannel(&htim15, &sConfigIC, TIM_CHANNEL_2) != HAL_OK) {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim15, &sMasterConfig) !=
+      HAL_OK) {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM15_Init 2 */
+
+  /* USER CODE END TIM15_Init 2 */
+}
+
 /**
  * @brief USART1 Initialization Function
  * @param None
@@ -324,11 +452,11 @@ static void MX_GPIO_Init(void) {
   HAL_GPIO_WritePin(PRECHARGE_CTRL_GPIO_Port, PRECHARGE_CTRL_Pin,
                     GPIO_PIN_RESET);
 
-  /*Configure GPIO pins : HV_MISMATCH_ERR_Pin RELAY_MISMATCH_ERR_Pin
+  /*Configure GPIO pins : HV_MISMATCH_ERR_Pin RELAY_MISMATCH_ERR_Pin IMD_OK_Pin
      RELAY_CONNECTION_ERR_Pin HV_ACTIVE_Pin NEG_AIR_CLOSED_Pin
      POS_AIR_CLOSED_Pin */
   GPIO_InitStruct.Pin = HV_MISMATCH_ERR_Pin | RELAY_MISMATCH_ERR_Pin |
-                        RELAY_CONNECTION_ERR_Pin | HV_ACTIVE_Pin |
+                        IMD_OK_Pin | RELAY_CONNECTION_ERR_Pin | HV_ACTIVE_Pin |
                         NEG_AIR_CLOSED_Pin | POS_AIR_CLOSED_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
   GPIO_InitStruct.Pull = GPIO_NOPULL;
@@ -376,6 +504,9 @@ void Error_Handler(void) {
   /* User can add his own implementation to report the HAL error return state */
   __disable_irq();
   while (1) {
+    ts_sm_set_relay_position(RELAY_NEG, 0);
+    ts_sm_set_relay_position(RELAY_POS, 0);
+    ts_sm_set_relay_position(RELAY_PRECHARGE, 0);
   }
   /* USER CODE END Error_Handler_Debug */
 }

Core/Src/shunt_monitoring.c

@@ -12,34 +12,54 @@ ShuntData shunt_data;
 
 void shunt_init() {
   shunt_data.current = 0;
-  shunt_data.voltage1 = 0;
+  shunt_data.voltage_veh = 0;
-  shunt_data.voltage2 = 0;
+  shunt_data.voltage_bat = 0;
   shunt_data.voltage3 = 0;
   shunt_data.busbartemp = 0;
   shunt_data.power = 0;
   shunt_data.energy = 0;
   shunt_data.current_counter = 0;
   shunt_data.last_message = 0;
+  shunt_data.last_current_message = 0;
 }
 
 void shunt_check() {
-  int is_error = shunt_data.current >= THRESH_OVERCURRENT;
+  int is_error = 0;
+  if (HAL_GetTick() - shunt_data.last_message > SHUNT_TIMEOUT) {
+    is_error = 1;
+    can_send_error(TS_ERRORKIND_SHUNT_TIMEOUT, 0);
+  } else if (shunt_data.current >= SHUNT_THRESH_OVERCURRENT ||
+             shunt_data.current <= -SHUNT_THRESH_OVERCURRENT_REGEN) {
+    is_error = 1;
+    can_send_error(TS_ERRORKIND_SHUNT_OVERCURRENT, 0);
+  } else if (shunt_data.busbartemp >= SHUNT_THRESH_OVERTEMP) {
+    is_error = 1;
+    can_send_error(TS_ERRORKIND_SHUNT_OVERTEMP, 0);
+  }
   ts_sm_set_error_source(TS_ERROR_SOURCE_SHUNT, is_error);
 }
 
 void shunt_handle_can_msg(uint16_t id, const uint8_t *data) {
+  shunt_data.last_message = HAL_GetTick();
+
   // All result messages contain a big-endian 6-byte integer
   uint64_t result = ftcan_unmarshal_unsigned(&data, 6);
 
   switch (id) {
   case CAN_ID_SHUNT_CURRENT:
     shunt_data.current = result;
+    if (shunt_data.last_current_message > 0) {
+      uint32_t now = HAL_GetTick();
+      float dt = (now - shunt_data.last_current_message) * 0.001f;
+      shunt_data.current_counter += shunt_data.current * dt;
+    }
+    shunt_data.last_current_message = HAL_GetTick();
     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;
@@ -51,7 +71,10 @@ void shunt_handle_can_msg(uint16_t id, const uint8_t *data) {
     shunt_data.power = result;
     break;
   case CAN_ID_SHUNT_CURRENT_COUNTER:
-    shunt_data.current_counter = result;
+    // TODO: Use this when we get the shunt to emit current counter data (the
+    // shunt apparently emits As, not mAs)
+
+    // shunt_data.current_counter = result * 1000;
     break;
   case CAN_ID_SHUNT_ENERGY_COUNTER:
     shunt_data.energy = result;

Core/Src/slave_monitoring.c

@@ -1,5 +1,6 @@
 #include "slave_monitoring.h"
 
+#include "can.h"
 #include "main.h"
 #include "ts_state_machine.h"
 
@@ -15,9 +16,29 @@ SlaveHandle slaves[N_SLAVES];
 uint16_t min_voltage;
 int16_t max_temp;
 
+static uint8_t slave_id_to_index[128] = {0xFF};
+
+static size_t get_slave_index(uint8_t slave_id) {
+  // Slave IDs are 7-bit, so we can use a 128-element array to map them to
+  // indices. 0xFF is used to mark unseen slave IDs, since the highest index we
+  // could need is N_SLAVES - 1 (i.e. 5).
+  static size_t next_slave_index = 0;
+  if (slave_id_to_index[slave_id] == 0xFF) {
+    if (next_slave_index >= N_SLAVES) {
+      // We've seen more than N_SLAVES slave IDs, this shouldn't happen.
+      Error_Handler();
+    }
+    slave_id_to_index[slave_id] = next_slave_index;
+    slaves[next_slave_index].id = slave_id;
+    next_slave_index++;
+  }
+  return slave_id_to_index[slave_id];
+}
+
 void slaves_init() {
+  memset(slave_id_to_index, 0xFF, sizeof(slave_id_to_index));
   for (int i = 0; i < N_SLAVES; i++) {
-    slaves[i].id = i;
+    slaves[i].id = 0xFF;
     slaves[i].error.kind = SLAVE_ERR_NONE;
     slaves[i].last_message = 0;
     slaves[i].min_voltage = 0;
@@ -40,14 +61,18 @@ void slaves_check() {
   uint16_t min_voltage_new = 0xFFFF;
   int16_t max_temp_new = 0xFFFF;
   for (int i = 0; i < N_SLAVES; i++) {
+    // Update timeout errors
     if (now - slaves[i].last_message >= SLAVE_TIMEOUT) {
       // Don't overwrite a different error kind
       if (slaves[i].error.kind == SLAVE_ERR_NONE) {
         slaves[i].error.kind = SLAVE_ERR_TIMEOUT;
+        can_send_error(TS_ERRORKIND_SLAVE_TIMEOUT, slaves[i].id);
       }
     } else if (slaves[i].error.kind == SLAVE_ERR_TIMEOUT) {
       slaves[i].error.kind = SLAVE_ERR_NONE;
     }
+
+    // Determine min/max
     if (slaves[i].min_voltage < min_voltage_new) {
       min_voltage_new = slaves[i].min_voltage;
     }
@@ -62,48 +87,47 @@ void slaves_check() {
   min_voltage = min_voltage_new;
   max_temp = max_temp_new;
 
-  if (any_slave_error) {
+  ts_sm_set_error_source(TS_ERROR_SOURCE_SLAVES, any_slave_error);
-    ts_sm_set_error_source(TS_ERROR_SOURCE_SLAVES, 1);
-  }
 }
 
 void slaves_handle_panic(const uint8_t *data) {
-  uint8_t slave_id = ftcan_unmarshal_unsigned(&data, 1);
+  const uint8_t **ptr = &data;
-  uint8_t error_kind = ftcan_unmarshal_unsigned(&data, 1);
+  uint8_t slave_id = ftcan_unmarshal_unsigned(ptr, 1);
+  uint8_t idx = get_slave_index(slave_id);
+  uint8_t error_kind = ftcan_unmarshal_unsigned(ptr, 1);
   switch (error_kind) {
   case SLAVE_PANIC_OT:
-    slaves[slave_id].error.kind = SLAVE_ERR_OT;
+    slaves[idx].error.kind = SLAVE_ERR_OT;
     break;
   case SLAVE_PANIC_UT:
-    slaves[slave_id].error.kind = SLAVE_ERR_UT;
+    slaves[idx].error.kind = SLAVE_ERR_UT;
     break;
   case SLAVE_PANIC_OV:
-    slaves[slave_id].error.kind = SLAVE_ERR_OV;
+    slaves[idx].error.kind = SLAVE_ERR_OV;
     break;
   case SLAVE_PANIC_UV:
-    slaves[slave_id].error.kind = SLAVE_ERR_UV;
+    slaves[idx].error.kind = SLAVE_ERR_UV;
     break;
   }
-  slaves[slave_id].error.data = ftcan_unmarshal_unsigned(&data, 4);
+  slaves[idx].error.data = ftcan_unmarshal_unsigned(&data, 4);
-  slaves[slave_id].last_message = HAL_GetTick();
+  slaves[idx].last_message = HAL_GetTick();
+  ts_sm_set_error_source(TS_ERROR_SOURCE_SLAVES, 1);
+  can_send_error(TS_ERRORKIND_SLAVE_PANIC, slave_id);
 }
 
 void slaves_handle_status(const uint8_t *data) {
   uint8_t slave_id = data[0] & 0x7F;
+  uint8_t idx = get_slave_index(slave_id);
   int error = data[0] & 0x80;
-  if (error) {
+  if (!error) {
-    if (slaves[slave_id].error.kind == SLAVE_ERR_NONE) {
+    slaves[idx].error.kind = SLAVE_ERR_NONE;
-      slaves[slave_id].error.kind = SLAVE_ERR_UNKNOWN;
   }
-  } else {
+  slaves[idx].soc = data[1];
-    slaves[slave_id].error.kind = SLAVE_ERR_NONE;
-  }
-  slaves[slave_id].soc = data[1];
   const uint8_t *ptr = &data[2];
-  slaves[slave_id].min_voltage = ftcan_unmarshal_unsigned(&ptr, 2);
+  slaves[idx].min_voltage = ftcan_unmarshal_unsigned(&ptr, 2);
-  slaves[slave_id].max_voltage = ftcan_unmarshal_unsigned(&ptr, 2);
+  slaves[idx].max_voltage = ftcan_unmarshal_unsigned(&ptr, 2);
-  slaves[slave_id].max_temp = ftcan_unmarshal_unsigned(&ptr, 2);
+  slaves[idx].max_temp = ftcan_unmarshal_unsigned(&ptr, 2);
-  slaves[slave_id].last_message = HAL_GetTick();
+  slaves[idx].last_message = HAL_GetTick();
 }
 
 void slaves_handle_log(const uint8_t *data) {

Core/Src/soc_estimation.c

@@ -1,14 +1,89 @@
 #include "soc_estimation.h"
 
+#include "shunt_monitoring.h"
+#include "slave_monitoring.h"
+
+#include "stm32f3xx_hal.h"
+
+#include <stddef.h>
 #include <stdint.h>
 
-uint8_t current_soc;
+#define SOC_ESTIMATION_NO_CURRENT_THRESH 200     // mA
+#define SOC_ESTIMATION_NO_CURRENT_TIME 100000    // ms
+#define SOC_ESTIMATION_BATTERY_CAPACITY 70300800 // mAs
+ocv_soc_pair_t OCV_SOC_PAIRS[] = {
+    {25000, 0.00f},  {29900, 3.97f},  {32300, 9.36f},  {33200, 12.60f},
+    {33500, 13.68f}, {34100, 20.15f}, {35300, 32.01f}, {38400, 66.53f},
+    {40100, 83.79f}, {40200, 90.26f}, {40400, 94.58f}, {41000, 98.89f},
+    {42000, 100.00f}};
+
+float current_soc;
+
+int current_was_flowing;
+uint32_t last_current_time;
+float soc_before_current;
+float mAs_before_current;
 
 void soc_init() {
   current_soc = 0;
-  // TODO
+  last_current_time = 0;
+  current_was_flowing = 1;
 }
 
 void soc_update() {
-  // TODO
+  uint32_t now = HAL_GetTick();
+  if (abs(shunt_data.current) >= SOC_ESTIMATION_NO_CURRENT_THRESH) {
+    last_current_time = now;
+    if (!current_was_flowing) {
+      soc_before_current = current_soc;
+      mAs_before_current = shunt_data.current_counter;
+    }
+    current_was_flowing = 1;
+  } else {
+    current_was_flowing = 0;
+  }
+
+  if (now - last_current_time >= SOC_ESTIMATION_NO_CURRENT_TIME ||
+      last_current_time == 0) {
+    // Assume we're measuring OCV if there's been no current for a while (or
+    // we've just turned on the battery).
+    current_soc = soc_for_ocv(min_voltage);
+  } else {
+    // Otherwise, use the current counter to update SoC
+    float as_delta = shunt_data.current_counter - mAs_before_current;
+    float soc_delta = as_delta / SOC_ESTIMATION_BATTERY_CAPACITY * 100;
+    current_soc = soc_before_current - soc_delta;
+  }
+}
+
+float soc_for_ocv(uint16_t ocv) {
+  size_t i = 0;
+  size_t array_length = sizeof(OCV_SOC_PAIRS) / sizeof(*OCV_SOC_PAIRS);
+  // Find the index of the first element with OCV greater than the target OCV
+  while (i < array_length && OCV_SOC_PAIRS[i].ocv <= ocv) {
+    i++;
+  }
+
+  // If the target OCV is lower than the smallest OCV in the array, return the
+  // first SOC value
+  if (i == 0) {
+    return OCV_SOC_PAIRS[0].soc;
+  }
+
+  // If the target OCV is higher than the largest OCV in the array, return the
+  // last SOC value
+  if (i == array_length) {
+    return OCV_SOC_PAIRS[array_length - 1].soc;
+  }
+
+  // Perform linear interpolation
+  uint16_t ocv1 = OCV_SOC_PAIRS[i - 1].ocv;
+  uint16_t ocv2 = OCV_SOC_PAIRS[i].ocv;
+  float soc1 = OCV_SOC_PAIRS[i - 1].soc;
+  float soc2 = OCV_SOC_PAIRS[i].soc;
+
+  float slope = (soc2 - soc1) / (ocv2 - ocv1);
+  float interpolated_soc = soc1 + slope * (ocv - ocv1);
+
+  return interpolated_soc;
 }

Core/Src/stm32f3xx_hal_msp.c

@@ -207,6 +207,149 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* hcan)
 
 }
 
+/**
+* @brief I2C MSP Initialization
+* This function configures the hardware resources used in this example
+* @param hi2c: I2C handle pointer
+* @retval None
+*/
+void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c)
+{
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(hi2c->Instance==I2C1)
+  {
+  /* USER CODE BEGIN I2C1_MspInit 0 */
+
+  /* USER CODE END I2C1_MspInit 0 */
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+    /**I2C1 GPIO Configuration
+    PA15     ------> I2C1_SCL
+    PB9     ------> I2C1_SDA
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_15;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+    GPIO_InitStruct.Alternate = GPIO_AF4_I2C1;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+    GPIO_InitStruct.Pin = GPIO_PIN_9;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+    GPIO_InitStruct.Alternate = GPIO_AF4_I2C1;
+    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+    /* Peripheral clock enable */
+    __HAL_RCC_I2C1_CLK_ENABLE();
+  /* USER CODE BEGIN I2C1_MspInit 1 */
+
+  /* USER CODE END I2C1_MspInit 1 */
+  }
+
+}
+
+/**
+* @brief I2C MSP De-Initialization
+* This function freeze the hardware resources used in this example
+* @param hi2c: I2C handle pointer
+* @retval None
+*/
+void HAL_I2C_MspDeInit(I2C_HandleTypeDef* hi2c)
+{
+  if(hi2c->Instance==I2C1)
+  {
+  /* USER CODE BEGIN I2C1_MspDeInit 0 */
+
+  /* USER CODE END I2C1_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_I2C1_CLK_DISABLE();
+
+    /**I2C1 GPIO Configuration
+    PA15     ------> I2C1_SCL
+    PB9     ------> I2C1_SDA
+    */
+    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_15);
+
+    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_9);
+
+  /* USER CODE BEGIN I2C1_MspDeInit 1 */
+
+  /* USER CODE END I2C1_MspDeInit 1 */
+  }
+
+}
+
+/**
+* @brief TIM_IC MSP Initialization
+* This function configures the hardware resources used in this example
+* @param htim_ic: TIM_IC handle pointer
+* @retval None
+*/
+void HAL_TIM_IC_MspInit(TIM_HandleTypeDef* htim_ic)
+{
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(htim_ic->Instance==TIM15)
+  {
+  /* USER CODE BEGIN TIM15_MspInit 0 */
+
+  /* USER CODE END TIM15_MspInit 0 */
+    /* Peripheral clock enable */
+    __HAL_RCC_TIM15_CLK_ENABLE();
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**TIM15 GPIO Configuration
+    PA2     ------> TIM15_CH1
+    */
+    GPIO_InitStruct.Pin = IMD_M_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF9_TIM15;
+    HAL_GPIO_Init(IMD_M_GPIO_Port, &GPIO_InitStruct);
+
+    /* TIM15 interrupt Init */
+    HAL_NVIC_SetPriority(TIM1_BRK_TIM15_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM1_BRK_TIM15_IRQn);
+  /* USER CODE BEGIN TIM15_MspInit 1 */
+
+  /* USER CODE END TIM15_MspInit 1 */
+  }
+
+}
+
+/**
+* @brief TIM_IC MSP De-Initialization
+* This function freeze the hardware resources used in this example
+* @param htim_ic: TIM_IC handle pointer
+* @retval None
+*/
+void HAL_TIM_IC_MspDeInit(TIM_HandleTypeDef* htim_ic)
+{
+  if(htim_ic->Instance==TIM15)
+  {
+  /* USER CODE BEGIN TIM15_MspDeInit 0 */
+
+  /* USER CODE END TIM15_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM15_CLK_DISABLE();
+
+    /**TIM15 GPIO Configuration
+    PA2     ------> TIM15_CH1
+    */
+    HAL_GPIO_DeInit(IMD_M_GPIO_Port, IMD_M_Pin);
+
+    /* TIM15 interrupt DeInit */
+    HAL_NVIC_DisableIRQ(TIM1_BRK_TIM15_IRQn);
+  /* USER CODE BEGIN TIM15_MspDeInit 1 */
+
+  /* USER CODE END TIM15_MspDeInit 1 */
+  }
+
+}
+
 /**
 * @brief UART MSP Initialization
 * This function configures the hardware resources used in this example

Core/Src/stm32f3xx_it.c

@@ -56,6 +56,7 @@
 
 /* External variables --------------------------------------------------------*/
 extern CAN_HandleTypeDef hcan;
+extern TIM_HandleTypeDef htim15;
 /* USER CODE BEGIN EV */
 
 /* USER CODE END EV */
@@ -74,6 +75,7 @@ void NMI_Handler(void)
   /* USER CODE BEGIN NonMaskableInt_IRQn 1 */
   while (1)
   {
+    Error_Handler();
   }
   /* USER CODE END NonMaskableInt_IRQn 1 */
 }
@@ -89,6 +91,7 @@ void HardFault_Handler(void)
   while (1)
   {
     /* USER CODE BEGIN W1_HardFault_IRQn 0 */
+    Error_Handler();
     /* USER CODE END W1_HardFault_IRQn 0 */
   }
 }
@@ -104,6 +107,7 @@ void MemManage_Handler(void)
   while (1)
   {
     /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
+    Error_Handler();
     /* USER CODE END W1_MemoryManagement_IRQn 0 */
   }
 }
@@ -119,6 +123,7 @@ void BusFault_Handler(void)
   while (1)
   {
     /* USER CODE BEGIN W1_BusFault_IRQn 0 */
+    Error_Handler();
     /* USER CODE END W1_BusFault_IRQn 0 */
   }
 }
@@ -134,6 +139,7 @@ void UsageFault_Handler(void)
   while (1)
   {
     /* USER CODE BEGIN W1_UsageFault_IRQn 0 */
+    Error_Handler();
     /* USER CODE END W1_UsageFault_IRQn 0 */
   }
 }
@@ -212,6 +218,20 @@ void USB_LP_CAN_RX0_IRQHandler(void)
   /* USER CODE END USB_LP_CAN_RX0_IRQn 1 */
 }
 
+/**
+  * @brief This function handles TIM1 break and TIM15 interrupts.
+  */
+void TIM1_BRK_TIM15_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM1_BRK_TIM15_IRQn 0 */
+
+  /* USER CODE END TIM1_BRK_TIM15_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim15);
+  /* USER CODE BEGIN TIM1_BRK_TIM15_IRQn 1 */
+
+  /* USER CODE END TIM1_BRK_TIM15_IRQn 1 */
+}
+
 /* USER CODE BEGIN 1 */
 
 /* USER CODE END 1 */

Core/Src/ts_state_machine.c

@@ -52,18 +52,26 @@ void ts_sm_update() {
 
 TSState ts_sm_update_inactive() {
   if (ts_state.target_state == TS_ACTIVE) {
+    if (sdc_closed) {
       precharge_95_reached_timestamp = 0;
       return TS_PRECHARGE;
+    } else {
+      return TS_DISCHARGE;
+    }
   } else if (ts_state.target_state == TS_CHARGING) {
+    if (sdc_closed) {
       charging_check_timestamp = HAL_GetTick();
       return TS_CHARGING_CHECK;
+    } else {
+      return TS_DISCHARGE;
+    }
   }
 
   return TS_INACTIVE;
 }
 
 TSState ts_sm_update_active() {
-  if (ts_state.target_state == TS_INACTIVE) {
+  if (ts_state.target_state == TS_INACTIVE || !sdc_closed) {
     discharge_begin_timestamp = HAL_GetTick();
     return TS_DISCHARGE;
   }
@@ -72,12 +80,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_closed) {
     discharge_begin_timestamp = HAL_GetTick();
     return TS_DISCHARGE;
   }
-  if (shunt_data.voltage2 > MIN_VEHICLE_SIDE_VOLTAGE &&
+  if (shunt_data.voltage_veh > MIN_VEHICLE_SIDE_VOLTAGE &&
-      shunt_data.voltage2 > 0.95 * shunt_data.voltage3) {
+      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;
@@ -106,20 +114,22 @@ TSState ts_sm_update_error() {
       no_error_since = now;
     } else if (now - no_error_since > NO_ERROR_TIME) {
       no_error_since = 0;
+      HAL_GPIO_WritePin(AMS_NERROR_GPIO_Port, AMS_NERROR_Pin, GPIO_PIN_SET);
       return TS_INACTIVE;
     }
   }
 
+  HAL_GPIO_WritePin(AMS_NERROR_GPIO_Port, AMS_NERROR_Pin, GPIO_PIN_RESET);
   return TS_ERROR;
 }
 
 TSState ts_sm_update_charging_check() {
-  if (ts_state.target_state == TS_INACTIVE) {
+  if (ts_state.target_state == TS_INACTIVE || !sdc_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 +140,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_closed) {
     discharge_begin_timestamp = HAL_GetTick();
     return TS_DISCHARGE;
   }

STM32Make.make

@@ -38,6 +38,7 @@ BUILD_DIR = build
 C_SOURCES =  \
 Core/Lib/can-halal/can-halal.c \
 Core/Src/can.c \
+Core/Src/imd_monitoring.c \
 Core/Src/main.c \
 Core/Src/shunt_monitoring.c \
 Core/Src/slave_monitoring.c \

ams-master-23.ioc

@@ -26,45 +26,51 @@ Mcu.CPN=STM32F302CBT6
 Mcu.Family=STM32F3
 Mcu.IP0=ADC2
 Mcu.IP1=CAN
-Mcu.IP2=NVIC
+Mcu.IP2=I2C1
-Mcu.IP3=RCC
+Mcu.IP3=NVIC
-Mcu.IP4=SYS
+Mcu.IP4=RCC
-Mcu.IP5=USART1
+Mcu.IP5=SYS
-Mcu.IPNb=6
+Mcu.IP6=TIM15
+Mcu.IP7=USART1
+Mcu.IPNb=8
 Mcu.Name=STM32F302C(B-C)Tx
 Mcu.Package=LQFP48
 Mcu.Pin0=PF0-OSC_IN
 Mcu.Pin1=PF1-OSC_OUT
-Mcu.Pin10=PB2
+Mcu.Pin10=PB0
-Mcu.Pin11=PB10
+Mcu.Pin11=PB1
-Mcu.Pin12=PB11
+Mcu.Pin12=PB2
-Mcu.Pin13=PB12
+Mcu.Pin13=PB10
-Mcu.Pin14=PB13
+Mcu.Pin14=PB11
-Mcu.Pin15=PB14
+Mcu.Pin15=PB12
-Mcu.Pin16=PB15
+Mcu.Pin16=PB13
-Mcu.Pin17=PA8
+Mcu.Pin17=PB14
-Mcu.Pin18=PA9
+Mcu.Pin18=PB15
-Mcu.Pin19=PA10
+Mcu.Pin19=PA8
 Mcu.Pin2=PA0
-Mcu.Pin20=PA11
+Mcu.Pin20=PA9
-Mcu.Pin21=PA12
+Mcu.Pin21=PA10
-Mcu.Pin22=PA13
+Mcu.Pin22=PA11
-Mcu.Pin23=PA14
+Mcu.Pin23=PA12
-Mcu.Pin24=PB3
+Mcu.Pin24=PA13
-Mcu.Pin25=PB4
+Mcu.Pin25=PA14
-Mcu.Pin26=PB5
+Mcu.Pin26=PA15
-Mcu.Pin27=PB6
+Mcu.Pin27=PB3
-Mcu.Pin28=PB7
+Mcu.Pin28=PB4
-Mcu.Pin29=PB8
+Mcu.Pin29=PB5
 Mcu.Pin3=PA1
-Mcu.Pin30=VP_SYS_VS_Systick
+Mcu.Pin30=PB6
-Mcu.Pin4=PA4
+Mcu.Pin31=PB7
-Mcu.Pin5=PA5
+Mcu.Pin32=PB8
-Mcu.Pin6=PA6
+Mcu.Pin33=PB9
-Mcu.Pin7=PA7
+Mcu.Pin34=VP_SYS_VS_Systick
-Mcu.Pin8=PB0
+Mcu.Pin4=PA2
-Mcu.Pin9=PB1
+Mcu.Pin5=PA3
-Mcu.PinsNb=31
+Mcu.Pin6=PA4
+Mcu.Pin7=PA5
+Mcu.Pin8=PA6
+Mcu.Pin9=PA7
+Mcu.PinsNb=35
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F302CBTx
@@ -80,6 +86,7 @@ NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false
+NVIC.TIM1_BRK_TIM15_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.USB_LP_CAN_RX0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 PA0.GPIOParameters=GPIO_Label
@@ -105,6 +112,16 @@ PA13.Signal=SYS_JTMS-SWDIO
 PA14.Locked=true
 PA14.Mode=Trace_Asynchronous_SW
 PA14.Signal=SYS_JTCK-SWCLK
+PA15.Mode=I2C
+PA15.Signal=I2C1_SCL
+PA2.GPIOParameters=GPIO_Label
+PA2.GPIO_Label=IMD_M
+PA2.Locked=true
+PA2.Signal=S_TIM15_CH1
+PA3.GPIOParameters=GPIO_Label
+PA3.GPIO_Label=IMD_OK
+PA3.Locked=true
+PA3.Signal=GPIO_Input
 PA4.GPIOParameters=GPIO_Label
 PA4.GPIO_Label=RELAY_CONNECTION_ERR
 PA4.Locked=true
@@ -188,6 +205,8 @@ PB8.GPIOParameters=GPIO_Label
 PB8.GPIO_Label=AMS_NERROR
 PB8.Locked=true
 PB8.Signal=GPIO_Output
+PB9.Mode=I2C
+PB9.Signal=I2C1_SDA
 PF0-OSC_IN.Mode=HSE-External-Oscillator
 PF0-OSC_IN.Signal=RCC_OSC_IN
 PF1-OSC_OUT.Mode=HSE-External-Oscillator
@@ -221,7 +240,7 @@ ProjectManager.StackSize=0x400
 ProjectManager.TargetToolchain=Makefile
 ProjectManager.ToolChainLocation=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_ADC2_Init-ADC2-false-HAL-true,4-MX_CAN_Init-CAN-false-HAL-true,5-MX_USART1_UART_Init-USART1-false-HAL-true
+ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_ADC2_Init-ADC2-false-HAL-true,4-MX_CAN_Init-CAN-false-HAL-true,5-MX_USART1_UART_Init-USART1-false-HAL-true,6-MX_I2C1_Init-I2C1-false-HAL-true,7-MX_TIM15_Init-TIM15-false-HAL-true
 RCC.ADC12outputFreq_Value=16000000
 RCC.AHBFreq_Value=16000000
 RCC.APB1Freq_Value=16000000
@@ -256,6 +275,10 @@ RCC.USART2Freq_Value=16000000
 RCC.USART3Freq_Value=16000000
 RCC.USBFreq_Value=16000000
 RCC.VCOOutput2Freq_Value=4000000
+SH.S_TIM15_CH1.0=TIM15_CH1,PWM_Input_1
+SH.S_TIM15_CH1.ConfNb=1
+TIM15.IPParameters=Prescaler
+TIM15.Prescaler=16000-1
 USART1.IPParameters=VirtualMode-Asynchronous
 USART1.VirtualMode-Asynchronous=VM_ASYNC
 VP_SYS_VS_Systick.Mode=SysTick