330 lines
8.9 KiB
C
330 lines
8.9 KiB
C
#include "state_machine.h"
|
|
#include "AMS_HighLevel.h"
|
|
#include "TMP1075.h"
|
|
#include "errors.h"
|
|
#include "main.h"
|
|
#include "stm32f3xx_hal.h"
|
|
#include <stdint.h>
|
|
|
|
StateHandle state;
|
|
int16_t RELAY_BAT_SIDE_VOLTAGE;
|
|
int16_t RELAY_ESC_SIDE_VOLTAGE;
|
|
int16_t CURRENT_MEASUREMENT;
|
|
uint8_t powerground_status;
|
|
|
|
static uint32_t timestamp;
|
|
|
|
void sm_init(){
|
|
state.current_state = STATE_INACTIVE;
|
|
state.target_state = STATE_INACTIVE;
|
|
state.error_source = 0;
|
|
}
|
|
|
|
void sm_update(){
|
|
sm_check_errors();
|
|
|
|
RELAY_BAT_SIDE_VOLTAGE = module.auxVoltages[0] * 12.42; // the calculation says the factor is 11.989. 12.42 yields the better result
|
|
RELAY_ESC_SIDE_VOLTAGE = module.auxVoltages[1] * 12.42;
|
|
CURRENT_MEASUREMENT = (module.auxVoltages[2] - 2496) * 300;
|
|
|
|
switch (state.current_state) {
|
|
case STATE_INACTIVE:
|
|
state.current_state = sm_update_inactive(); // monitor only
|
|
break;
|
|
case STATE_PRECHARGE:
|
|
state.current_state = sm_update_precharge(); // set PRECHARGE and turn on cooling at 50% or such
|
|
break;
|
|
case STATE_READY:
|
|
state.current_state = sm_update_ready(); // keep cooling at 50%, get ready to turn on powerground
|
|
break;
|
|
case STATE_ACTIVE:
|
|
state.current_state = sm_update_active(); // set PRECHARGE and turn on cooling at 50% or such
|
|
break;
|
|
case STATE_DISCHARGE:
|
|
state.current_state = sm_update_discharge(); // open the main relay, keep PRECHARGE closed
|
|
break;
|
|
case STATE_CHARGING_PRECHARGE:
|
|
state.current_state = sm_update_charging_precharge();
|
|
break;
|
|
case STATE_CHARGING:
|
|
state.current_state = sm_update_charging(); // monitor and turn on cooling if needed.
|
|
break;
|
|
case STATE_ERROR:
|
|
state.current_state = sm_update_error(); // enter the correct ERROR state
|
|
break;
|
|
}
|
|
|
|
sm_set_relay_positions(state.current_state);
|
|
state.target_state = state.current_state;
|
|
}
|
|
|
|
State sm_update_inactive(){
|
|
switch (state.target_state) {
|
|
case STATE_PRECHARGE:
|
|
return STATE_PRECHARGE;
|
|
case STATE_CHARGING_PRECHARGE:
|
|
return STATE_CHARGING_PRECHARGE;
|
|
default:
|
|
return STATE_INACTIVE;
|
|
}
|
|
}
|
|
|
|
State sm_update_precharge(){
|
|
switch (state.target_state) {
|
|
case STATE_INACTIVE: // if CAN Signal 0000 0000 then immidiete shutdown
|
|
return STATE_DISCHARGE;
|
|
case STATE_PRECHARGE:
|
|
if (RELAY_BAT_SIDE_VOLTAGE-RELAY_ESC_SIDE_VOLTAGE < 100){
|
|
PWM_set_throttle();
|
|
return STATE_READY;
|
|
}
|
|
return STATE_PRECHARGE;
|
|
case STATE_DISCHARGE:
|
|
return STATE_DISCHARGE;
|
|
default:
|
|
return STATE_PRECHARGE;
|
|
}
|
|
}
|
|
|
|
State sm_update_ready(){
|
|
switch (state.target_state) {
|
|
case STATE_ACTIVE: // if CAN Signal 1100 0000 then turn on powerground
|
|
return STATE_ACTIVE;
|
|
case STATE_DISCHARGE: // if CAN Signal 0000 0000 then shutdown
|
|
return STATE_DISCHARGE;
|
|
default:
|
|
return STATE_READY;
|
|
}
|
|
}
|
|
|
|
State sm_update_active(){
|
|
switch (state.target_state) {
|
|
case STATE_READY: // if CAN Signal 1000 0000 then turn oof powerground but stay ready
|
|
return STATE_READY;
|
|
case STATE_DISCHARGE: // if CAN Signal 0000 0000 then shutdown
|
|
return STATE_DISCHARGE;
|
|
default:
|
|
return STATE_ACTIVE;
|
|
}
|
|
}
|
|
|
|
State sm_update_discharge(){
|
|
switch (state.target_state) {
|
|
case STATE_DISCHARGE:
|
|
return (RELAY_ESC_SIDE_VOLTAGE < 5000) ? STATE_INACTIVE : STATE_DISCHARGE;
|
|
case STATE_PRECHARGE: // if CAN Signal 1000 0000 then get ready
|
|
return STATE_PRECHARGE;
|
|
default:
|
|
return STATE_DISCHARGE;
|
|
}
|
|
}
|
|
|
|
State sm_update_charging_precharge(){
|
|
switch (state.target_state) {
|
|
case STATE_CHARGING:
|
|
return STATE_CHARGING;
|
|
case STATE_DISCHARGE:
|
|
return STATE_DISCHARGE;
|
|
default:
|
|
return STATE_CHARGING_PRECHARGE;
|
|
}
|
|
}
|
|
|
|
State sm_update_charging(){
|
|
switch (state.target_state) {
|
|
case STATE_DISCHARGE:
|
|
return STATE_DISCHARGE;
|
|
default:
|
|
return STATE_CHARGING;
|
|
}
|
|
}
|
|
|
|
|
|
State sm_update_error(){
|
|
switch (state.target_state) {
|
|
case STATE_DISCHARGE:
|
|
return STATE_DISCHARGE;
|
|
default:
|
|
return STATE_ERROR;
|
|
}
|
|
}
|
|
|
|
void sm_set_relay_positions(State current_state){
|
|
switch (state.current_state) {
|
|
case STATE_INACTIVE:
|
|
sm_set_relay(RELAY_MAIN, 0);
|
|
sm_set_relay(RELAY_PRECHARGE, 0);
|
|
break;
|
|
case STATE_PRECHARGE:
|
|
sm_set_relay(RELAY_MAIN, 0);
|
|
sm_set_relay(RELAY_PRECHARGE, 1);
|
|
break;
|
|
case STATE_READY:
|
|
sm_set_relay(RELAY_MAIN, 1);
|
|
sm_set_relay(RELAY_PRECHARGE, 0);
|
|
break;
|
|
case STATE_ACTIVE:
|
|
sm_set_relay(RELAY_MAIN, 1);
|
|
sm_set_relay(RELAY_PRECHARGE, 0);
|
|
break;
|
|
case STATE_DISCHARGE:
|
|
sm_set_relay(RELAY_MAIN, 0);
|
|
sm_set_relay(RELAY_PRECHARGE, 0);
|
|
break;
|
|
case STATE_CHARGING_PRECHARGE:
|
|
sm_set_relay(RELAY_MAIN, 0);
|
|
sm_set_relay(RELAY_PRECHARGE, 1);
|
|
break;
|
|
case STATE_CHARGING:
|
|
sm_set_relay(RELAY_MAIN, 1);
|
|
sm_set_relay(RELAY_PRECHARGE, 0);
|
|
break;
|
|
case STATE_ERROR:
|
|
sm_set_relay(RELAY_MAIN, 0);
|
|
sm_set_relay(RELAY_PRECHARGE, 0);
|
|
break;
|
|
}
|
|
}
|
|
|
|
void sm_set_relay(Relay relay, bool closed){
|
|
GPIO_PinState state = closed ? GPIO_PIN_SET : GPIO_PIN_RESET;
|
|
switch (relay) {
|
|
case RELAY_MAIN:
|
|
HAL_GPIO_WritePin(RELAY_ENABLE_GPIO_Port, RELAY_ENABLE_Pin, state);
|
|
relay_closed = closed;
|
|
break;
|
|
case RELAY_PRECHARGE:
|
|
HAL_GPIO_WritePin(PRECHARGE_ENABLE_GPIO_Port, PRECHARGE_ENABLE_Pin, state);
|
|
precharge_closed = closed;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void sm_check_charging(){
|
|
if (RELAY_BAT_SIDE_VOLTAGE < RELAY_ESC_SIDE_VOLTAGE && timestamp == 0)
|
|
timestamp = HAL_GetTick() + 5000;
|
|
if (timestamp < HAL_GetTick())
|
|
state.target_state = STATE_CHARGING_PRECHARGE;
|
|
}
|
|
|
|
/* returns the ID and temperature of the hottest cell */
|
|
void sm_check_battery_temperature(int8_t *id, int16_t *temp){
|
|
for (int i = 0; i < N_TEMP_SENSORS; i++) {
|
|
if (tmp1075_temps[i] > *temp){
|
|
*id = i;
|
|
*temp = tmp1075_temps[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
int16_t sm_return_cell_temperature(int id){
|
|
return tmp1075_temps[id];
|
|
}
|
|
|
|
int16_t sm_return_cell_voltage(int id){
|
|
return module.cellVoltages[id];
|
|
}
|
|
|
|
void sm_handle_ams_in(const uint8_t *data){
|
|
switch (data[0]) {
|
|
case 0x00:
|
|
if (state.current_state != STATE_INACTIVE){
|
|
state.target_state = STATE_DISCHARGE;
|
|
}
|
|
break;
|
|
case 0x01:
|
|
if (state.target_state == STATE_INACTIVE || state.target_state == STATE_DISCHARGE){
|
|
state.target_state = STATE_PRECHARGE;
|
|
} else if (state.target_state == STATE_ACTIVE){
|
|
state.target_state = STATE_READY;
|
|
}
|
|
break;
|
|
case 0x02:
|
|
if (state.current_state == STATE_READY || state.current_state == STATE_ACTIVE){
|
|
PWM_powerground_control(data[1]);
|
|
state.target_state = STATE_ACTIVE; // READY -> ACTIVE
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
void sm_set_error(ErrorKind error_kind, bool is_errored){}
|
|
|
|
#warning TODO: add error checking for everything here
|
|
void sm_check_errors(){
|
|
switch (error_data.data_kind) {
|
|
case SEK_OVERTEMP:
|
|
case SEK_UNDERTEMP:
|
|
case SEK_TOO_FEW_TEMPS:
|
|
state.error_type.temperature_error = 1;
|
|
break;
|
|
case SEK_OVERVOLT:
|
|
case SEK_UNDERVOLT:
|
|
case SEK_OPENWIRE:
|
|
state.error_type.voltage_error = 1;
|
|
break;
|
|
case SEK_EEPROM_ERR:
|
|
state.error_type.eeprom_error = 1;
|
|
break;
|
|
case SEK_INTERNAL_BMS_TIMEOUT:
|
|
state.error_type.bms_timeout = 1;
|
|
break;
|
|
case SEK_INTERNAL_BMS_CHECKSUM_FAIL:
|
|
case SEK_INTERNAL_BMS_OVERTEMP:
|
|
case SEK_INTERNAL_BMS_FAULT:
|
|
state.error_type.bms_fault = 1;
|
|
break;
|
|
}
|
|
|
|
if (1){
|
|
state.error_type.current_error = 1;
|
|
}
|
|
|
|
if (1){
|
|
state.error_type.current_sensor_missing = 1;
|
|
}
|
|
|
|
if (RELAY_BAT_SIDE_VOLTAGE < 30000){
|
|
state.error_type.voltage_error = 1;
|
|
}
|
|
|
|
if (1){
|
|
state.error_type.voltage_missing = 1;
|
|
}
|
|
}
|
|
|
|
void sm_test_cycle_states(){
|
|
RELAY_BAT_SIDE_VOLTAGE = module.auxVoltages[0];
|
|
RELAY_ESC_SIDE_VOLTAGE = module.auxVoltages[1];
|
|
CURRENT_MEASUREMENT = module.auxVoltages[2];
|
|
sm_set_relay_positions(state.current_state);
|
|
|
|
if (timestamp > HAL_GetTick())
|
|
return;
|
|
switch (state.current_state) {
|
|
case STATE_INACTIVE:
|
|
state.current_state = STATE_PRECHARGE;
|
|
timestamp = HAL_GetTick() + 30000;
|
|
PWM_powerground_control(0);
|
|
break;
|
|
case STATE_PRECHARGE:
|
|
state.current_state = STATE_READY;
|
|
timestamp = HAL_GetTick() + 10000;
|
|
break;
|
|
case STATE_READY:
|
|
state.current_state = STATE_ACTIVE;
|
|
timestamp = HAL_GetTick() + 10000;
|
|
break;
|
|
case STATE_ACTIVE:
|
|
state.current_state = STATE_DISCHARGE;
|
|
timestamp = HAL_GetTick() + 10000;
|
|
PWM_powerground_control(1);
|
|
break;
|
|
case STATE_DISCHARGE:
|
|
state.current_state = STATE_INACTIVE;
|
|
timestamp = HAL_GetTick() + 10000;
|
|
break;
|
|
}
|
|
|
|
state.target_state = state.current_state;
|
|
} |