FaSTTUBe/ams-master
56
1
Fork 0
Code Issues 2 Pull Requests Packages Projects Releases Wiki Activity

added master23 code files

Browse Source
This commit is contained in:
Lene Marquardt 2025-04-07 14:20:00 +02:00
parent f09566ce49
commit 49a12a4b2c
14 changed files with 942 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
AMS_Master_Code/Core/Inc/can.h Normal file
View File

@ -0,0 +1,31 @@
#ifndef INC_CAN_H
#define INC_CAN_H
#include "stm32h7xx_hal.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_AMS_ERROR 0x00C
#define CAN_ID_SLAVE_STATUS_BASE 0x080
#define CAN_ID_AMS_SIGNALS 0x090
#define CAN_ID_SLAVE_LOG 0x4F4
#define CAN_ID_SHUNT_BASE 0x520
#define CAN_ID_SHUNT_CURRENT 0x521
#define CAN_ID_SHUNT_VOLTAGE1 0x522
#define CAN_ID_SHUNT_VOLTAGE2 0x523
#define CAN_ID_SHUNT_VOLTAGE3 0x524
#define CAN_ID_SHUNT_TEMP 0x525
#define CAN_ID_SHUNT_POWER 0x526
#define CAN_ID_SHUNT_CURRENT_COUNTER 0x527
#define CAN_ID_SHUNT_ENERGY_COUNTER 0x528
void can_init(FDCAN_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);
#endif // INC_CAN_H

36
AMS_Master_Code/Core/Inc/imd_monitoring.h Normal file
View File

@ -0,0 +1,36 @@
#ifndef INC_IMD_MONITORING_H
#define INC_IMD_MONITORING_H
#include <stdint.h>
#include "stm32h7xx_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

32
AMS_Master_Code/Core/Inc/shunt_monitoring.h Normal file
View File

@ -0,0 +1,32 @@
#ifndef INC_SHUNT_MONITORING_H
#define INC_SHUNT_MONITORING_H
#include <stdint.h>
#include "stm32h7xx_hal.h"
#define SHUNT_TIMEOUT 300 // ms
#define SHUNT_THRESH_OVERCURRENT 300000 // mA
#define SHUNT_THRESH_OVERTEMP 1000 // 1/10 °C
typedef struct {
int32_t current; // mA
int32_t voltage_bat; // mV
int32_t voltage_veh; // mV
int32_t voltage3; // mV
int32_t busbartemp;
int32_t power;
int32_t energy;
float current_counter; // mAs
uint32_t last_message;
uint32_t last_current_message;
} ShuntData;
extern ShuntData shunt_data;
void shunt_init();
void shunt_check();
void shunt_handle_can_msg(uint16_t id, const uint8_t *data);
#endif // INC_SHUNT_MONITORING_H

18
AMS_Master_Code/Core/Inc/soc_estimation.h Normal file
View File

@ -0,0 +1,18 @@
#ifndef INC_SOC_ESTIMATION_H
#define INC_SOC_ESTIMATION_H
#include <stdint.h>
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

12
AMS_Master_Code/Core/Inc/status_led.h Normal file
View File

@ -0,0 +1,12 @@
#ifndef INC_STATUS_LED_H
#define INC_STATUS_LED_H
#include "ts_state_machine.h"
void status_led_init();
void status_led_state(TSState state, TSErrorKind error);
void set_led_internal_error();
#endif // INC_STATUS_LED_H

76
AMS_Master_Code/Core/Inc/ts_state_machine.h Normal file
View File

@ -0,0 +1,76 @@
#ifndef INC_TS_STATE_MACHINE_H
#define INC_TS_STATE_MACHINE_H
#include "stm32h7xx_hal.h"
#include <stdint.h>
#include <stdbool.h>
// 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
// 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
#define NO_ERROR_TIME 1000 // ms
// Time to wait for charger voltage before going to TS_ERROR
#define MAX_CHARGING_CHECK_DURATION 2000 // ms
// Time to wait between closing relays
#define RELAY_CLOSE_WAIT 10 // ms
typedef enum {
TS_INACTIVE,
TS_ACTIVE,
TS_PRECHARGE,
TS_DISCHARGE,
TS_ERROR,
TS_CHARGING_CHECK,
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;
typedef enum {
TS_ERROR_SOURCE_SHUNT = (1 << 0),
TS_ERROR_SOURCE_SLAVES = (1 << 1)
} TSErrorSource;
typedef struct {
TSState current_state;
TSState target_state;
uint16_t error_source; // TSErrorSource (bitmask)
uint16_t error_type; // TSErrorKind
} TSStateHandle;
extern TSStateHandle ts_state;
void ts_sm_init();
void ts_sm_update();
TSState ts_sm_update_inactive();
TSState ts_sm_update_active();
TSState ts_sm_update_precharge();
TSState ts_sm_update_discharge();
TSState ts_sm_update_error();
TSState ts_sm_update_charging_check();
TSState ts_sm_update_charging();
typedef enum { RELAY_NEG, RELAY_POS, RELAY_PRECHARGE } Relay;
void ts_sm_set_relay_positions(TSState state);
void ts_sm_set_relay_position(Relay relay, int closed);
void ts_sm_check_close_wait(int *is_closed, int should_close);
void ts_sm_handle_ams_in(const uint8_t *data);
void ts_sm_set_error_source(TSErrorSource source, TSErrorKind error_type, bool is_errored);
#endif // INC_TS_STATE_MACHINE_H

8
AMS_Master_Code/Core/Inc/util.h Normal file
View File

@ -0,0 +1,8 @@
#ifndef INC_UTIL_H
#define INC_UTIL_H
#include <stdint.h>
void set_error_led();
#endif // INC_UTIL_H

72
AMS_Master_Code/Core/Src/can.c Normal file
View File

@ -0,0 +1,72 @@
#include "can.h"
#include "imd_monitoring.h"
#include "main.h"
#include "shunt_monitoring.h"
#include "slave_monitoring.h"
#include "soc_estimation.h"
#include "ts_state_machine.h"
#include "can-halal.h"
#include <math.h>
#include <stdint.h>
void can_init(FDCAN_HandleTypeDef *handle) {
ftcan_init(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_BASE, 0xFF0);
ftcan_add_filter(CAN_ID_SLAVE_LOG, 0xFFF);
}
HAL_StatusTypeDef can_send_status() {
uint8_t data[8];
data[0] = ts_state.current_state | (sdc_closed << 7);
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;
}
HAL_StatusTypeDef ret = ftcan_transmit(CAN_ID_AMS_STATUS, data, sizeof(data));
if (ret != HAL_OK) {
return ret;
}
data[0] = (sdc_closed_nodelay << 0) | (ts_error << 1) | (hv_active << 2) |
(neg_air_closed << 3) | (pos_air_closed << 4) |
(precharge_closed << 5) | (precharge_opened << 6);
return ftcan_transmit(CAN_ID_AMS_SIGNALS, data, 1);
}
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;
}
switch (id) {
case CAN_ID_SLAVE_PANIC:
slaves_handle_panic(data);
break;
case CAN_ID_SLAVE_LOG:
slaves_handle_log(data);
break;
case CAN_ID_AMS_IN:
ts_sm_handle_ams_in(data);
break;
}
}

86
AMS_Master_Code/Core/Src/imd_monitoring.c Normal file
View File

@ -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;
}
}
}

86
AMS_Master_Code/Core/Src/shunt_monitoring.c Normal file
View File

@ -0,0 +1,86 @@
#include "shunt_monitoring.h"
#include <string.h>
#include "can.h"
#include "ts_state_machine.h"
#include "util.h"
#include "can-halal.h"
ShuntData shunt_data;
void shunt_init() {
shunt_data.current = 0;
shunt_data.voltage_veh = 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 = 0;
TSErrorKind error_type = TS_ERRORKIND_NONE;
if (HAL_GetTick() - shunt_data.last_message > SHUNT_TIMEOUT) {
is_error = 1;
error_type = TS_ERRORKIND_SHUNT_TIMEOUT;
can_send_error(TS_ERRORKIND_SHUNT_TIMEOUT, 0);
} else if (shunt_data.current >= SHUNT_THRESH_OVERCURRENT) {
is_error = 1;
error_type = TS_ERRORKIND_SHUNT_OVERCURRENT;
can_send_error(TS_ERRORKIND_SHUNT_OVERCURRENT, 0);
} else if (shunt_data.busbartemp >= SHUNT_THRESH_OVERTEMP) {
is_error = 1;
error_type = TS_ERRORKIND_SHUNT_OVERTEMP;
can_send_error(TS_ERRORKIND_SHUNT_OVERTEMP, 0);
}
ts_sm_set_error_source(TS_ERROR_SOURCE_SHUNT, error_type, 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.voltage_bat = result;
break;
case CAN_ID_SHUNT_VOLTAGE2:
shunt_data.voltage_veh = result;
break;
case CAN_ID_SHUNT_VOLTAGE3:
shunt_data.voltage3 = result;
break;
case CAN_ID_SHUNT_TEMP:
shunt_data.busbartemp = result;
break;
case CAN_ID_SHUNT_POWER:
shunt_data.power = result;
break;
case CAN_ID_SHUNT_CURRENT_COUNTER:
// 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;
break;
}
}

90
AMS_Master_Code/Core/Src/soc_estimation.c Normal file
View File

@ -0,0 +1,90 @@
#include "soc_estimation.h"
#include "shunt_monitoring.h"
#include "slave_monitoring.h"
#include "stm32h7xx_hal.h"
#include <stddef.h>
#include <stdint.h>
#include <stdlib.h>
#define SOC_ESTIMATION_NO_CURRENT_THRESH 200 // mA
#define SOC_ESTIMATION_NO_CURRENT_TIME 100000 // ms
#define SOC_ESTIMATION_BATTERY_CAPACITY 64800000 // mAs
ocv_soc_pair_t OCV_SOC_PAIRS[] = {
{2500, 0.00f}, {2990, 3.97f}, {3230, 9.36f}, {3320, 12.60f},
{3350, 13.68f}, {3410, 20.15f}, {3530, 32.01f}, {3840, 66.53f},
{4010, 83.79f}, {4020, 90.26f}, {4040, 94.58f}, {4100, 98.89f},
{4200, 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;
last_current_time = 0;
current_was_flowing = 1;
}
void soc_update() {
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;
}

142
AMS_Master_Code/Core/Src/status_led.c Normal file
View File

@ -0,0 +1,142 @@
#include "status_led.h"
#include "main.h"
#include "stm32h7xx_hal.h"
#include "ts_state_machine.h"
#include <stdint.h>
#warning enable the second LED
typedef struct {
uint8_t red : 1;
uint8_t green : 1;
uint8_t blue : 1;
} LedColor;
typedef enum {
LED_1,
LED_2
} LedId;
uint32_t count = 0;
bool main_led = false;
LedColor led_1 = (LedColor) {.red = 0, .green = 0, .blue = 0};
LedColor led_2 = (LedColor) {.red = 0, .green = 0, .blue = 0};
#define ITER_COUNT 20
#define INTERN_ERROR_COUNT 7500
static void set_led_color_hw(LedColor color) {
GPIO_PinState red = color.red ? GPIO_PIN_RESET : GPIO_PIN_SET;
GPIO_PinState green = color.green ? GPIO_PIN_RESET : GPIO_PIN_SET;
GPIO_PinState blue = color.blue ? GPIO_PIN_RESET : GPIO_PIN_SET;
HAL_GPIO_WritePin(STATUS_LED_R_GPIO_Port, STATUS_LED_R_Pin, red);
HAL_GPIO_WritePin(STATUS_LED_G_GPIO_Port, STATUS_LED_G_Pin, green);
HAL_GPIO_WritePin(STATUS_LED_B_GPIO_Port, STATUS_LED_B_Pin, blue);
}
static void set_led_color(LedId id, LedColor color) {
switch (id) {
case LED_1:
led_1 = color;
break;
case LED_2:
led_2 = color;
break;
}
set_led_color_hw(main_led ? led_1 : led_2);
count++;
if (count > ITER_COUNT) {
count = 0;
main_led = !main_led;
}
}
void set_led_internal_error() {
led_1 = (LedColor) {.red = 1, .green = 0, .blue = 0};
led_2 = (LedColor) {.red = 0, .green = 0, .blue = 0};
set_led_color_hw(main_led ? led_1 : led_2);
count++;
if (count > INTERN_ERROR_COUNT) {
count = 0;
main_led = !main_led;
}
}
/**
* @brief Sets the status LEDs to the given state
*
* @param state The state to set the LEDs to
*
* State -> Color mapping:
* TS_INACTIVE -> LED_1: green, LED_2: off
* TS_ACTIVE -> LED_1: blue, LED_2: off
* TS_PRECHARGE -> LED_1: blue, LED_2: cyan
* TS_DISCHARGE -> LED_1: blue, LED_2: magenta
* TS_ERROR -> LED_1: red, LED_2: <see below>
* TS_CHARGING_CHECK -> LED_1: yellow, LED_2: cyan
* TS_CHARGING -> LED_1: yellow, LED_2: off
*
* Error -> LED_2 mapping:
* TS_ERRORKIND_NONE -> off
* TS_ERRORKIND_SLAVE_TIMEOUT -> blue
* TS_ERRORKIND_SLAVE_PANIC -> magenta
* TS_ERRORKIND_SHUNT_TIMEOUT -> green
* TS_ERRORKIND_SHUNT_OVERCURRENT -> yellow
* TS_ERRORKIND_SHUNT_OVERTEMP -> red
*/
void status_led_state(TSState state, TSErrorKind error) {
switch (state) {
case TS_INACTIVE:
set_led_color(LED_1, (LedColor) {.red = 0, .green = 1, .blue = 0});
set_led_color(LED_2, (LedColor) {.red = 0, .green = 0, .blue = 0});
break;
case TS_ACTIVE:
set_led_color(LED_1, (LedColor) {.red = 0, .green = 0, .blue = 1});
set_led_color(LED_2, (LedColor) {.red = 0, .green = 0, .blue = 0});
break;
case TS_PRECHARGE:
set_led_color(LED_1, (LedColor) {.red = 0, .green = 0, .blue = 1});
set_led_color(LED_2, (LedColor) {.red = 0, .green = 1, .blue = 1});
break;
case TS_DISCHARGE:
set_led_color(LED_1, (LedColor) {.red = 0, .green = 0, .blue = 1});
set_led_color(LED_2, (LedColor) {.red = 1, .green = 0, .blue = 1});
break;
case TS_ERROR:
set_led_color(LED_1, (LedColor) {.red = 1, .green = 0, .blue = 0});
switch (error) {
case TS_ERRORKIND_NONE:
set_led_color(LED_2, (LedColor) {.red = 0, .green = 0, .blue = 0});
break;
case TS_ERRORKIND_SLAVE_TIMEOUT:
set_led_color(LED_2, (LedColor) {.red = 0, .green = 0, .blue = 1});
break;
case TS_ERRORKIND_SLAVE_PANIC:
set_led_color(LED_2, (LedColor) {.red = 1, .green = 0, .blue = 1});
break;
case TS_ERRORKIND_SHUNT_TIMEOUT:
set_led_color(LED_2, (LedColor) {.red = 0, .green = 1, .blue = 0});
break;
case TS_ERRORKIND_SHUNT_OVERCURRENT:
set_led_color(LED_2, (LedColor) {.red = 1, .green = 1, .blue = 0});
break;
case TS_ERRORKIND_SHUNT_OVERTEMP:
set_led_color(LED_2, (LedColor) {.red = 1, .green = 0, .blue = 0});
break;
}
break;
case TS_CHARGING_CHECK:
set_led_color(LED_1, (LedColor) {.red = 1, .green = 1, .blue = 0});
set_led_color(LED_2, (LedColor) {.red = 0, .green = 1, .blue = 1});
break;
case TS_CHARGING:
set_led_color(LED_1, (LedColor) {.red = 1, .green = 1, .blue = 0});
set_led_color(LED_2, (LedColor) {.red = 0, .green = 0, .blue = 0});
break;
}
}

245
AMS_Master_Code/Core/Src/ts_state_machine.c Normal file
View File

@ -0,0 +1,245 @@
#include "ts_state_machine.h"
#include "main.h"
#include "shunt_monitoring.h"
#include "status_led.h"
#include "stm32h7xx_hal.h"
#include <stdint.h>
TSStateHandle ts_state;
static uint32_t precharge_95_reached_timestamp = 0;
static uint32_t charging_check_timestamp = 0;
static uint32_t discharge_begin_timestamp = 0;
static uint32_t precharge_opened_timestamp = 0;
void ts_sm_init() {
ts_state.current_state = TS_INACTIVE;
ts_state.target_state = TS_INACTIVE;
ts_state.error_source = 0;
}
void ts_sm_update() {
if (ts_state.error_source) {
ts_state.current_state = TS_ERROR;
}
switch (ts_state.current_state) {
case TS_INACTIVE:
ts_state.current_state = ts_sm_update_inactive();
break;
case TS_ACTIVE:
ts_state.current_state = ts_sm_update_active();
break;
case TS_PRECHARGE:
ts_state.current_state = ts_sm_update_precharge();
break;
case TS_DISCHARGE:
ts_state.current_state = ts_sm_update_discharge();
break;
case TS_ERROR:
ts_state.current_state = ts_sm_update_error();
break;
case TS_CHARGING_CHECK:
ts_state.current_state = ts_sm_update_charging_check();
break;
case TS_CHARGING:
ts_state.current_state = ts_sm_update_charging();
break;
}
ts_sm_set_relay_positions(ts_state.current_state);
status_led_state(ts_state.current_state, (TSErrorKind)ts_state.error_type);
}
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 || !sdc_closed) {
discharge_begin_timestamp = HAL_GetTick();
return TS_DISCHARGE;
}
if (!precharge_closed && precharge_opened_timestamp == 0) {
precharge_opened_timestamp = HAL_GetTick();
} else if (precharge_opened_timestamp != 0 &&
HAL_GetTick() - precharge_opened_timestamp > 100) {
precharge_opened = 1;
}
return TS_ACTIVE;
}
TSState ts_sm_update_precharge() {
if (ts_state.target_state == TS_INACTIVE || !sdc_closed) {
discharge_begin_timestamp = HAL_GetTick();
return TS_DISCHARGE;
}
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;
} else if ((now - precharge_95_reached_timestamp) >=
PRECHARGE_95_DURATION) {
precharge_95_reached_timestamp = 0;
precharge_opened_timestamp = 0;
precharge_opened = 0;
return TS_ACTIVE;
}
}
return TS_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;
}
}
TSState ts_sm_update_error() {
static uint32_t no_error_since = 0;
if (ts_state.error_source == 0) {
uint32_t now = HAL_GetTick();
if (no_error_since == 0) {
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 || !sdc_closed) {
discharge_begin_timestamp = HAL_GetTick();
return TS_DISCHARGE;
}
if (shunt_data.voltage_veh > shunt_data.voltage_bat) {
return TS_CHARGING;
} else if (HAL_GetTick() - charging_check_timestamp >
MAX_CHARGING_CHECK_DURATION) {
return TS_ERROR;
}
return TS_CHARGING_CHECK;
}
TSState ts_sm_update_charging() {
if (ts_state.target_state == TS_INACTIVE || !sdc_closed) {
discharge_begin_timestamp = HAL_GetTick();
return TS_DISCHARGE;
}
if (shunt_data.current < 0) {
return TS_ERROR;
}
return TS_CHARGING;
}
void ts_sm_set_relay_positions(TSState state) {
switch (state) {
case TS_INACTIVE:
case TS_DISCHARGE:
case TS_ERROR:
ts_sm_set_relay_position(RELAY_NEG, 0);
ts_sm_set_relay_position(RELAY_POS, 0);
ts_sm_set_relay_position(RELAY_PRECHARGE, 0);
break;
case TS_ACTIVE:
case TS_CHARGING:
ts_sm_set_relay_position(RELAY_NEG, 1);
ts_sm_set_relay_position(RELAY_POS, 1);
ts_sm_set_relay_position(RELAY_PRECHARGE, !precharge_opened);
// TODO: Open precharge relay after a while
break;
case TS_PRECHARGE:
case TS_CHARGING_CHECK:
ts_sm_set_relay_position(RELAY_NEG, 1);
ts_sm_set_relay_position(RELAY_POS, 0);
ts_sm_set_relay_position(RELAY_PRECHARGE, 1);
break;
}
}
void ts_sm_set_relay_position(Relay relay, int closed) {
static int neg_closed = 0;
static int pos_closed = 0;
static int precharge_closed = 0;
GPIO_PinState state = closed ? GPIO_PIN_SET : GPIO_PIN_RESET;
switch (relay) {
case RELAY_NEG:
ts_sm_check_close_wait(&neg_closed, closed);
HAL_GPIO_WritePin(NEG_AIR_CTRL_GPIO_Port, NEG_AIR_CTRL_Pin, state);
break;
case RELAY_POS:
ts_sm_check_close_wait(&pos_closed, closed);
HAL_GPIO_WritePin(POS_AIR_CTRL_GPIO_Port, POS_AIR_CTRL_Pin, state);
break;
case RELAY_PRECHARGE:
ts_sm_check_close_wait(&precharge_closed, closed);
HAL_GPIO_WritePin(PRECHARGE_CTRL_GPIO_Port, PRECHARGE_CTRL_Pin, state);
break;
}
}
void ts_sm_check_close_wait(int *is_closed, int should_close) {
static uint32_t last_close_timestamp = 0;
if (should_close != *is_closed) {
*is_closed = should_close;
if (should_close) {
uint32_t dt = HAL_GetTick() - last_close_timestamp;
if (dt < RELAY_CLOSE_WAIT) {
HAL_Delay(RELAY_CLOSE_WAIT - dt);
}
last_close_timestamp = HAL_GetTick();
}
}
}
void ts_sm_handle_ams_in(const uint8_t *data) {
if (data[0] & 0x01) {
ts_state.target_state = TS_ACTIVE;
} else {
ts_state.target_state = TS_INACTIVE;
}
}
void ts_sm_set_error_source(TSErrorSource source, TSErrorKind error_type,
bool is_errored) {
if (is_errored) {
ts_state.error_source |= source;
ts_state.error_type = error_type;
} else {
ts_state.error_source &= ~source;
ts_state.error_type = ~error_type;
}
}

8
AMS_Master_Code/Core/Src/util.c Normal file
View File

@ -0,0 +1,8 @@
#include "util.h"
#include "main.h"
#include "stm32h7xx_hal.h"
void set_error_led() {
#warning "check callers of this function"
//HAL_GPIO_WritePin(STATUS2_GPIO_Port, STATUS2_Pin, GPIO_PIN_SET);
}