From 4375bfce4856004b6c07f313a73b7d3487731a28 Mon Sep 17 00:00:00 2001 From: hamza Date: Mon, 8 Jul 2024 23:43:41 +0300 Subject: [PATCH] V1.7 --- CHANGELOG.txt | 2 + Core/Inc/state_machine.h | 19 ++- Core/Src/can.c | 20 +-- Core/Src/soc_estimation.c | 4 +- Core/Src/state_machine.c | 277 +++++++++++++++++++------------------- 5 files changed, 160 insertions(+), 162 deletions(-) diff --git a/CHANGELOG.txt b/CHANGELOG.txt index c75ba07..6f0c9d5 100644 --- a/CHANGELOG.txt +++ b/CHANGELOG.txt @@ -44,3 +44,5 @@ V1.7 - changed the CAN message a bit - added soc_estimation.c soc_estimation.h - added MIN/MAX_CELL_VOLTAGE for AMS_HighLevel +- cleaned up state_machine code +- changed the format of the CAN message. \ No newline at end of file diff --git a/Core/Inc/state_machine.h b/Core/Inc/state_machine.h index 3e54708..7e9a47c 100644 --- a/Core/Inc/state_machine.h +++ b/Core/Inc/state_machine.h @@ -61,15 +61,23 @@ typedef struct { ErrorKind error_type; // TSErrorKind } StateHandle; +typedef enum { RELAY_MAIN, RELAY_PRECHARGE } Relay; + extern StateHandle state; extern int32_t RELAY_BAT_SIDE_VOLTAGE; extern int32_t RELAY_ESC_SIDE_VOLTAGE; extern int32_t CURRENT_MEASUREMENT; extern uint8_t powerground_status; - void sm_init(); void sm_update(); +void sm_handle_ams_in(const uint8 *data); +void sm_precharge_discharge_manager(); +void sm_calibrate_powerground(); +void sm_balancing(); + +void sm_check_errors(); +void sm_set_error_source(); State sm_update_inactive(); State sm_update_precharge(); @@ -80,21 +88,12 @@ State sm_update_charging_precharge(); State sm_update_charging(); State sm_update_error(); -typedef enum { RELAY_MAIN, RELAY_PRECHARGE } Relay; void sm_set_relay_positions(State state); void sm_set_relay(Relay relay, bool closed); - void sm_check_battery_temperature(int8_t* id, int16_t* temp); int16_t sm_return_cell_temperature(int id); int16_t sm_return_cell_voltage(int id); -void sm_calibrate_powerground(); -void sm_precharge_discharge_manager(); - -void sm_handle_ams_in(const uint8 *data); -void sm_check_errors(); -void sm_set_error(ErrorKind error_kind, bool is_errored); void sm_test_cycle_states(); -void sm_error_source(); #endif /* "INC_STATE_MACHINE_H" */ \ No newline at end of file diff --git a/Core/Src/can.c b/Core/Src/can.c index 0020c59..e8d6b2a 100644 --- a/Core/Src/can.c +++ b/Core/Src/can.c @@ -62,16 +62,20 @@ void can_handle_send_status() { uint8_t data[8] = {}; int8_t id_highest_temp = -1; int16_t highest_temp = INT16_MIN; + int8_t id_lowest_volt = -1; + int16_t lowest_volt = INT16_MIN; sm_check_battery_temperature(&id_highest_temp, &highest_temp); data[0] = ((state.current_state << 4) | (powerground_status >> 4)); // 1 bit emptyy | 3 bit state | 4 bit powerground data[1] = ((powerground_status << 4) | (state.error_source >> 4)); // 4 bit powerground | 4 bit error data[2] = ((state.error_source << 4) | (0)); // 4 bit error | 4 bit state of charge - data[3] = ((RELAY_BAT_SIDE_VOLTAGE >> 8)); // 8 bit battery voltage - data[4] = ((RELAY_BAT_SIDE_VOLTAGE >> 0)); - data[5] = ((CURRENT_MEASUREMENT) / 1000); - data[6] = (/*(CURRENT_MEASUREMENT & 0x000F) |*/ (highest_temp >> 12)); - data[7] = ((highest_temp) >> 4); + data[3] = ((RELAY_BAT_SIDE_VOLTAGE / 1000)); // 8 bit battery voltage + data[4] = ((RELAY_ESC_SIDE_VOLTAGE / 1000)); + //data[5] = (() / 1000); + data[6] = ((CURRENT_MEASUREMENT / 1000)); + data[7] = ((highest_temp) >> 8); + //data[6] = (module.cellVoltages[7] >> 8); + //data[7] = (module.cellVoltages[7]); //data[7] = state.error_source; ftcan_transmit(CAN_ID_OUT, data, sizeof(data)); /* @@ -128,9 +132,9 @@ to the sm_handle_ams_in() which handles the state machine transition. This function recieves a command from the Autobox with the CAN ID of 0x501. with format of: data[0] = target state - 0x0 STATE_INACTIVE | disconnect power to the ESC of powerground. Send it to return the mvbms to idle/monitoring mode. If data[1] != 0 -> assume bad CAN message. - 0x1 STATE_READY | conneect power to the ESC of powerground and but with no PWM signal. If data[1] != 0 -> assume bad CAN message. - 0x2 STATE_ACTIVE | activate powerground at (data[1]) percent. If data[1] > 100 -> assume bad CAN message. + 0x00 STATE_INACTIVE | disconnect power to the ESC of powerground. Send it to return the mvbms to idle/monitoring mode. If data[1] != 0 -> assume bad CAN message. + 0x01 STATE_READY | conneect power to the ESC of powerground and but with no PWM signal. If data[1] != 0 -> assume bad CAN message. + 0x02 STATE_ACTIVE | activate powerground at (data[1]) percent. If data[1] > 100 -> assume bad CAN message. allowed transitions: STATE_INACTIVE -> STATE_READY diff --git a/Core/Src/soc_estimation.c b/Core/Src/soc_estimation.c index 32f3184..524f55f 100644 --- a/Core/Src/soc_estimation.c +++ b/Core/Src/soc_estimation.c @@ -49,10 +49,10 @@ void soc_update() { 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); + //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 as_delta = CURRENT_MEASUREMENT - mAs_before_current; float soc_delta = as_delta / SOC_ESTIMATION_BATTERY_CAPACITY * 100; current_soc = soc_before_current + soc_delta; } diff --git a/Core/Src/state_machine.c b/Core/Src/state_machine.c index e57791e..57d102e 100644 --- a/Core/Src/state_machine.c +++ b/Core/Src/state_machine.c @@ -6,6 +6,7 @@ */ #include "state_machine.h" +#include "ADBMS_Abstraction.h" #include "AMS_HighLevel.h" #include "PWM_control.h" #include "TMP1075.h" @@ -89,6 +90,138 @@ void sm_update(){ state.target_state = state.current_state; } +void sm_handle_ams_in(const uint8_t *data){ + CAN_timer = HAL_GetTick() + CAN_TIMEOUT; + 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){ + PWM_powerground_control(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; + #warning implement this + case 0xF1: // EEPROM + 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_calibrate_powerground(){ + if (powerground_calibration_stage != 4 && state.current_state == STATE_READY){ + 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; + } + } +} + +#warning TODO: add error checking for everything here +void sm_check_errors(){ + 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); + } else if (state.current_state == STATE_ERROR){ + 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: @@ -162,6 +295,8 @@ State sm_update_charging(){ case STATE_DISCHARGE: return STATE_DISCHARGE; default: + //amsConfigBalancing((1 << 7), 0xF); + //amsStartBalancing(0);e return STATE_CHARGING; } } @@ -236,135 +371,6 @@ void sm_check_battery_temperature(int8_t *id, int16_t *temp){ } } -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_calibrate_powerground(){ - if (powerground_calibration_stage != 4 && state.current_state == STATE_READY){ - 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_handle_ams_in(const uint8_t *data){ - CAN_timer = HAL_GetTick() + CAN_TIMEOUT; - 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){ - PWM_powerground_control(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; - #warning implement this - case 0xF1: // EEPROM - break; - case 0xFF: // EMERGENCY SHUTDOWN - state.current_state = STATE_DISCHARGE; - state.target_state = STATE_ERROR; - break; - } -} - -void sm_set_error(ErrorKind error_kind, bool is_errored){} - -/* -bool sm_is_errored(){ - return 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; -} -*/ - -#warning TODO: add error checking for everything here -void sm_check_errors(){ - 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); - } else if (state.current_state == STATE_ERROR){ - state.target_state = STATE_INACTIVE; - } - sm_error_source(); -} - int16_t sm_return_cell_temperature(int id){ return tmp1075_temps[id]; } int16_t sm_return_cell_voltage(int id){ return module.cellVoltages[id]; } @@ -407,17 +413,4 @@ void sm_test_cycle_states(){ } state.target_state = state.current_state; -} - -void sm_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); } \ No newline at end of file