540 lines
16 KiB
C
540 lines
16 KiB
C
/*
|
|
* state_machine.h
|
|
*
|
|
* Created on: 07.07.2024
|
|
* Author: Hamza
|
|
*/
|
|
|
|
#include <state_machine.h>
|
|
#include <stdint.h>
|
|
#include "ADBMS_Abstraction.h"
|
|
#include "AMS_HighLevel.h"
|
|
#include "PWM_control.h"
|
|
#include "can.h"
|
|
#include "eeprom.h"
|
|
#include "stm32f3xx_hal.h"
|
|
|
|
// 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_DURATION 8000 // ms
|
|
// Time to wait for discharge
|
|
#define DISCHARGE_DURATION 8000 // ms
|
|
// waiting time between to eeprom writes
|
|
#define EEPROM_WRITE_FREQ 1000
|
|
// how delay between steps of 5 -> 15ms * (100/5) = 300ms
|
|
#define POWERGROUND_SOFTSTART_INCREMENT_DELAY 15
|
|
// after errors are cleared wait for ERROR_LATCH_TIME ms before returning to inactive mode
|
|
#define ERROR_LATCH_TIME 10000 //ms
|
|
|
|
/*
|
|
10
|
|
20
|
|
30
|
|
40
|
|
50
|
|
60
|
|
70
|
|
80
|
|
90
|
|
100
|
|
*/
|
|
|
|
bool programming_mode;
|
|
bool debugging_mode;
|
|
|
|
StateHandle state;
|
|
int32_t RELAY_BAT_SIDE_VOLTAGE;
|
|
int32_t RELAY_ESC_SIDE_VOLTAGE;
|
|
int32_t CURRENT_MEASUREMENT;
|
|
bool CURRENT_MEASUREMENT_ON;
|
|
bool balancing_state;
|
|
float base_offset = 0;
|
|
|
|
uint32_t error_timer;
|
|
uint32_t precharge_timer;
|
|
uint32_t discharge_timer;
|
|
uint32_t balancing_timer;
|
|
uint32_t eeprom_timer;
|
|
|
|
uint32_t powerground_softstart_timer;
|
|
uint32_t powerground_calibration_timer;
|
|
uint8_t powerground_calibration_stage;
|
|
|
|
uint8_t current_powerground_status;
|
|
uint8_t target_powerground_status;
|
|
|
|
static uint32_t timestamp;
|
|
|
|
void sm_init(){
|
|
state.current_state = STATE_INACTIVE;
|
|
state.target_state = STATE_INACTIVE;
|
|
state.error_source = 0;
|
|
precharge_timer = discharge_timer = powerground_calibration_timer = error_timer = eeprom_timer = balancing_timer = 0;
|
|
programming_mode = 0;
|
|
debugging_mode = 0;
|
|
balancing_state = 0;
|
|
}
|
|
|
|
void sm_update(){
|
|
|
|
CURRENT_MEASUREMENT = (module.auxVoltages[0] > 2495) ? (module.auxVoltages[0] - (2495.0)) * (300.0) : 0;
|
|
CURRENT_MEASUREMENT_ON = (module.auxVoltages[1] > 2400);
|
|
RELAY_ESC_SIDE_VOLTAGE = module.auxVoltages[2] * 15.19;
|
|
RELAY_BAT_SIDE_VOLTAGE = module.auxVoltages[3] * 15.19; // the calculation says the factor is 11. 11.711 yields the better result
|
|
|
|
if (can_timeout_timer < HAL_GetTick())
|
|
state.current_state = state.target_state = STATE_INACTIVE;
|
|
/*
|
|
if (eeprom_timer < HAL_GetTick()){
|
|
eeprom_write_status();
|
|
eeprom_timer = HAL_GetTick() + EEPROM_WRITE_FREQ;
|
|
}
|
|
*/
|
|
can_handle_send_status();
|
|
can_handle_send_log();
|
|
sm_check_errors();
|
|
sm_precharge_discharge_manager();
|
|
//sm_calibrate_powerground();
|
|
sm_balancing();
|
|
sm_powerground_manager();
|
|
tmp1075_measure();
|
|
status_led_update();
|
|
soc_update();
|
|
|
|
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
|
|
break;
|
|
case STATE_READY:
|
|
state.current_state = sm_update_ready();
|
|
break;
|
|
case STATE_ACTIVE:
|
|
state.current_state = sm_update_active();
|
|
break;
|
|
case STATE_DISCHARGE:
|
|
state.current_state = sm_update_discharge();
|
|
break;
|
|
case STATE_CHARGING_PRECHARGE:
|
|
state.current_state = sm_update_charging_precharge();
|
|
break;
|
|
case STATE_CHARGING:
|
|
state.current_state = sm_update_charging();
|
|
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;
|
|
}
|
|
|
|
void sm_handle_ams_in(const uint8_t *data){
|
|
if (programming_mode == 1 && (state.current_state == STATE_READY || state.current_state == STATE_ACTIVE)){ PWM_powerground_control(data[1]); }
|
|
|
|
switch (data[0]) {
|
|
case 0x00:
|
|
if (state.current_state != STATE_INACTIVE){
|
|
state.target_state = STATE_DISCHARGE;
|
|
PWM_powerground_control(255);
|
|
}
|
|
break;
|
|
case 0x01:
|
|
if (state.target_state == STATE_INACTIVE || state.target_state == STATE_DISCHARGE){
|
|
state.target_state = STATE_PRECHARGE;
|
|
PWM_powerground_control(0);
|
|
} else if (state.target_state == STATE_ACTIVE){
|
|
state.target_state = STATE_READY;
|
|
PWM_powerground_control(0);
|
|
}
|
|
break;
|
|
case 0x02:
|
|
if (state.current_state == STATE_READY || state.current_state == STATE_ACTIVE){
|
|
target_powerground_status = data[1];
|
|
state.target_state = STATE_ACTIVE; // READY -> ACTIVE
|
|
}
|
|
break;
|
|
case 0xF0:
|
|
if (state.current_state == STATE_INACTIVE){
|
|
state.target_state = STATE_CHARGING_PRECHARGE;
|
|
}
|
|
break;
|
|
case 0xF1: // EEPROM
|
|
if (state.current_state == STATE_INACTIVE)
|
|
can_handle_dump();
|
|
break;
|
|
case 0xFF: // EMERGENCY SHUTDOWN
|
|
state.current_state = STATE_DISCHARGE;
|
|
state.target_state = STATE_ERROR;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void sm_precharge_discharge_manager(){
|
|
|
|
if (state.current_state != STATE_PRECHARGE && state.target_state == STATE_PRECHARGE){
|
|
precharge_timer = HAL_GetTick() + PRECHARGE_DURATION;
|
|
} else if (state.current_state == STATE_PRECHARGE && precharge_timer < HAL_GetTick()) {
|
|
state.target_state = STATE_READY;
|
|
precharge_timer = 0;
|
|
}
|
|
|
|
if (state.current_state != STATE_CHARGING_PRECHARGE && state.target_state == STATE_CHARGING_PRECHARGE){
|
|
precharge_timer = HAL_GetTick() + PRECHARGE_DURATION;
|
|
} else if (state.current_state == STATE_CHARGING_PRECHARGE && precharge_timer < HAL_GetTick()) {
|
|
state.target_state = STATE_CHARGING;
|
|
precharge_timer = 0;
|
|
}
|
|
|
|
if (state.current_state != STATE_DISCHARGE && state.target_state == STATE_DISCHARGE){
|
|
discharge_timer = HAL_GetTick() + DISCHARGE_DURATION;
|
|
} else if (state.current_state == STATE_DISCHARGE && discharge_timer < HAL_GetTick()) {
|
|
state.target_state = STATE_INACTIVE;
|
|
discharge_timer = 0;
|
|
}
|
|
|
|
}
|
|
|
|
void sm_powerground_manager(){
|
|
if (current_powerground_status == target_powerground_status)
|
|
return;
|
|
if ( current_powerground_status > 100 || target_powerground_status > 100){ //something went wrong
|
|
PWM_powerground_control(255);
|
|
current_powerground_status = target_powerground_status= 0;
|
|
return;
|
|
}
|
|
|
|
if (powerground_softstart_timer < HAL_GetTick()){
|
|
if (current_powerground_status < target_powerground_status){
|
|
current_powerground_status += 5;
|
|
PWM_powerground_softcontrol();
|
|
powerground_softstart_timer = HAL_GetTick() + POWERGROUND_SOFTSTART_INCREMENT_DELAY;
|
|
} else if (current_powerground_status > target_powerground_status) {
|
|
current_powerground_status -= 5;
|
|
PWM_powerground_softcontrol();
|
|
powerground_softstart_timer = HAL_GetTick() + POWERGROUND_SOFTSTART_INCREMENT_DELAY;
|
|
}
|
|
}
|
|
}
|
|
|
|
void sm_calibrate_powerground(){
|
|
if (powerground_calibration_stage != 4 && state.current_state == STATE_PRECHARGE){
|
|
switch (powerground_calibration_stage) {
|
|
case 0:
|
|
powerground_calibration_timer = HAL_GetTick() + 0;
|
|
powerground_calibration_stage = 1;
|
|
return;
|
|
case 1:
|
|
if (powerground_calibration_timer < HAL_GetTick()){
|
|
powerground_calibration_timer = HAL_GetTick() + 2000;
|
|
powerground_calibration_stage = 2;
|
|
PWM_powerground_control(100);
|
|
}
|
|
return;
|
|
case 2:
|
|
if (powerground_calibration_timer < HAL_GetTick()){
|
|
powerground_calibration_timer = HAL_GetTick() + 1000;
|
|
powerground_calibration_stage = 3;
|
|
PWM_powerground_control(0);
|
|
}
|
|
return;
|
|
case 3:
|
|
if (powerground_calibration_timer < HAL_GetTick()){
|
|
powerground_calibration_stage = 4;
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
void sm_balancing(){
|
|
uint32_t channels = 0b11111111111111111111111111111100;
|
|
if (balancing_timer < HAL_GetTick()){
|
|
if (balancing_state){
|
|
balancing_timer = HAL_GetTick() + 30000;
|
|
|
|
uint8_t id_cell_lowest_voltage = 0;
|
|
uint8_t num_of_cells_to_balance = 0;
|
|
uint32_t channels = 0;
|
|
|
|
for (int i = 0; i < 13; i++) {
|
|
if (module.cellVoltages[i] < module.cellVoltages[id_cell_lowest_voltage])
|
|
id_cell_lowest_voltage = i;
|
|
}
|
|
|
|
for (int i = 0; i < 13; i++) {
|
|
if (module.cellVoltages[i] - 20 > module.cellVoltages[id_cell_lowest_voltage]){
|
|
channels |= 1 << i;
|
|
num_of_cells_to_balance++;
|
|
}
|
|
}
|
|
|
|
if (num_of_cells_to_balance == 0){
|
|
amsStopBalancing();
|
|
balancing_state = 0;
|
|
return;
|
|
}
|
|
|
|
amsConfigBalancing(channels, 0xF);
|
|
amsStartBalancing(0xF);
|
|
balancing_state = 1;
|
|
} else {
|
|
balancing_timer = HAL_GetTick() + 1000;
|
|
balancing_state = 0;
|
|
amsStopBalancing();
|
|
}
|
|
}
|
|
}
|
|
|
|
void sm_program_powerground(){
|
|
if (programming_mode == 0)
|
|
return;
|
|
PWM_powerground_control(100);
|
|
state.current_state = state.target_state = STATE_ACTIVE;
|
|
while (1) {
|
|
can_handle_send_status();
|
|
}
|
|
}
|
|
|
|
void sm_eeprom_write_status(){
|
|
if (eeprom_timer < HAL_GetTick()){
|
|
eeprom_write_status();
|
|
eeprom_timer = HAL_GetTick() + EEPROM_WRITE_FREQ;
|
|
}
|
|
}
|
|
|
|
void sm_check_errors(){
|
|
if (programming_mode == 1 || debugging_mode == 1) {return;} // to disable error checking
|
|
state.error_type.temperature_error = (error_data.error_sources & (1 << 0) || error_data.error_sources & (1 << 1) || error_data.error_sources & (1 << 4)) ? 1 : 0;
|
|
state.error_type.voltage_error = (error_data.error_sources & (1 << 2)|| error_data.error_sources & (1 << 3)|| error_data.error_sources & (1 << 5) || RELAY_BAT_SIDE_VOLTAGE < 30000) ? 1 : 0;
|
|
state.error_type.bms_timeout = (error_data.error_sources & (1 << 7)) ? 1 : 0;
|
|
state.error_type.bms_fault = (error_data.error_sources & (1 << 8) || error_data.error_sources & (1 << 10) || error_data.error_sources & (1 << 9)) ? 1 : 0;
|
|
//SEK_EEPROM_ERR: state.error_type.eeprom_error = 1;
|
|
//state.error_type.current_error = (powerground_status > 10 && CURRENT_MEASUREMENT < 500) ? 1 : 0;
|
|
state.error_type.current_sensor_missing = (!CURRENT_MEASUREMENT_ON) ? 1 : 0;
|
|
state.error_type.voltage_missing = (RELAY_BAT_SIDE_VOLTAGE < 1000) ? 1 : 0;
|
|
|
|
|
|
if (state.error_type.current_error == 1 || state.error_type.current_sensor_missing == 1 || //state.error_type.eeprom_error == 1 ||
|
|
state.error_type.state_transition_fail == 1 || state.error_type.temperature_error == 1 || state.error_type.voltage_error == 1 ||
|
|
state.error_type.voltage_missing == 1 || state.error_type.bms_fault == 1 || state.error_type.bms_timeout == 1)
|
|
{
|
|
if (state.current_state != STATE_INACTIVE && state.current_state != STATE_ERROR)
|
|
state.current_state = STATE_DISCHARGE;
|
|
state.target_state = STATE_ERROR;
|
|
PWM_powerground_control(255);
|
|
error_timer = HAL_GetTick() + ERROR_LATCH_TIME;
|
|
} else if (state.current_state == STATE_ERROR && error_timer < HAL_GetTick()){
|
|
state.target_state = STATE_INACTIVE;
|
|
}
|
|
sm_set_error_source();
|
|
}
|
|
|
|
void sm_set_error_source(){
|
|
state.error_source = 0;
|
|
state.error_source |= (state.error_type.bms_timeout << 0);
|
|
state.error_source |= (state.error_type.bms_fault << 1);
|
|
state.error_source |= (state.error_type.temperature_error << 2);
|
|
state.error_source |= (state.error_type.current_error << 3);
|
|
|
|
state.error_source |= (state.error_type.current_sensor_missing << 4);
|
|
state.error_source |= (state.error_type.voltage_error << 5);
|
|
state.error_source |= (state.error_type.voltage_missing << 6);
|
|
state.error_source |= (state.error_type.state_transition_fail << 7);
|
|
}
|
|
|
|
State sm_update_inactive(){
|
|
switch (state.target_state) {
|
|
case STATE_PRECHARGE:
|
|
return STATE_PRECHARGE;
|
|
case STATE_CHARGING_PRECHARGE:
|
|
return STATE_CHARGING_PRECHARGE;
|
|
case STATE_ERROR:
|
|
return STATE_ERROR;
|
|
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_READY:
|
|
return STATE_READY;
|
|
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_INACTIVE:
|
|
return STATE_INACTIVE;
|
|
case STATE_ERROR:
|
|
return STATE_ERROR;
|
|
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:
|
|
currentAMSState = AMSIDLE;
|
|
return STATE_DISCHARGE;
|
|
default:
|
|
currentAMSState = AMSCHARGING;
|
|
return STATE_CHARGING;
|
|
}
|
|
}
|
|
|
|
State sm_update_error(){
|
|
switch (state.target_state) {
|
|
case STATE_DISCHARGE:
|
|
return STATE_DISCHARGE;
|
|
case STATE_INACTIVE:
|
|
return STATE_INACTIVE;
|
|
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, 1);
|
|
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);
|
|
break;
|
|
case RELAY_PRECHARGE:
|
|
HAL_GPIO_WritePin(PRECHARGE_ENABLE_GPIO_Port, PRECHARGE_ENABLE_Pin, state);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* returns the ID and temperature of the hottest cell */
|
|
void sm_check_battery_temperature(uint8_t *id, uint16_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_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(10);
|
|
break;
|
|
case STATE_DISCHARGE:
|
|
state.current_state = STATE_INACTIVE;
|
|
timestamp = HAL_GetTick() + 10000;
|
|
break;
|
|
case STATE_CHARGING_PRECHARGE:
|
|
case STATE_CHARGING:
|
|
case STATE_ERROR:
|
|
break;
|
|
}
|
|
|
|
state.target_state = state.current_state;
|
|
} |