added master23 code files
This commit is contained in:
parent
f09566ce49
commit
49a12a4b2c
31
AMS_Master_Code/Core/Inc/can.h
Normal file
31
AMS_Master_Code/Core/Inc/can.h
Normal 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
36
AMS_Master_Code/Core/Inc/imd_monitoring.h
Normal 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
32
AMS_Master_Code/Core/Inc/shunt_monitoring.h
Normal 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
18
AMS_Master_Code/Core/Inc/soc_estimation.h
Normal 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
12
AMS_Master_Code/Core/Inc/status_led.h
Normal 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
76
AMS_Master_Code/Core/Inc/ts_state_machine.h
Normal 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
8
AMS_Master_Code/Core/Inc/util.h
Normal 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
72
AMS_Master_Code/Core/Src/can.c
Normal 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
86
AMS_Master_Code/Core/Src/imd_monitoring.c
Normal 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
86
AMS_Master_Code/Core/Src/shunt_monitoring.c
Normal 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
90
AMS_Master_Code/Core/Src/soc_estimation.c
Normal 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
142
AMS_Master_Code/Core/Src/status_led.c
Normal 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
245
AMS_Master_Code/Core/Src/ts_state_machine.c
Normal 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
8
AMS_Master_Code/Core/Src/util.c
Normal 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);
|
||||||
|
}
|
Loading…
x
Reference in New Issue
Block a user