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