ams-master-23/Core/Src/ts_state_machine.c

222 lines
5.7 KiB
C

#include "ts_state_machine.h"
#include "main.h"
#include "shunt_monitoring.h"
#include "stm32f3xx_hal.h"
#include "stm32f3xx_hal_gpio.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;
void ts_sm_init() {
ts_state.current_state = TS_INACTIVE;
ts_state.target_state = TS_INACTIVE;
ts_state.error = 0;
}
void ts_sm_update() {
if (ts_state.error) {
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);
}
TSState ts_sm_update_inactive() {
if (ts_state.target_state == TS_ACTIVE) {
precharge_95_reached_timestamp = 0;
return TS_PRECHARGE;
} else if (ts_state.target_state == TS_CHARGING) {
charging_check_timestamp = HAL_GetTick();
return TS_CHARGING_CHECK;
}
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;
}
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;
return TS_ACTIVE;
}
}
return TS_PRECHARGE;
}
TSState ts_sm_update_discharge() {
if (HAL_GetTick() - discharge_begin_timestamp >= DISCHARGE_DURATION) {
return TS_INACTIVE;
} else {
return TS_DISCHARGE;
}
}
TSState ts_sm_update_error() {
static uint32_t no_error_since = 0;
if (ts_state.error == 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, 1);
// 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(uint32_t flag, int state) {
if (state) {
ts_state.error |= flag;
} else {
ts_state.error &= ~flag;
}
}