diff --git a/Core/Inc/state_machine.h b/Core/Inc/state_machine.h index e1326f2..64f657d 100644 --- a/Core/Inc/state_machine.h +++ b/Core/Inc/state_machine.h @@ -1,7 +1,17 @@ -//int errorcode[2] = {0,0}; 1 Bit per error +#ifndef INC_STATE_MACHINE_H +#define INC_STATE_MACHINE_H + #include #include +#include "ADBMS_LL_Driver.h" +#include "AMS_HighLevel.h" +#include "PWM_control.h" +#include "stm32f3xx_hal.h" +#include "ADBMS_Abstraction.h" +#include "main.h" +#include "can.h" + // Minimum vehicle side voltage to exit precharge #define MIN_VEHICLE_SIDE_VOLTAGE 150000 // mV // Time to wait after reaching 95% of battery voltage before exiting precharge @@ -16,19 +26,19 @@ // Time to wait between closing relays #define RELAY_CLOSE_WAIT 10 // ms -typedef enum { // 7 states -> 3 bit. valid transitions: (all could transition to error) - STATE_INACTIVE, // INACTIVE -> PRECHARGE, CHARGING, ERROR - STATE_PRECHARGE, // PRECHARGE -> INACTIVE, READY, DISCHARGE, ERROR - STATE_READY, // READY -> ACTIVE, DISCHARGE, ERROR - STATE_ACTIVE, // ACTIVE -> READY, DISCHARGE, ERROR - STATE_DISCHARGE, // DISCHARGE -> INACTIVE, PRECHARGE, ERROR - STATE_CHARGING, // CHARGING -> INACTIVE, DISCHARGE, ERROR - STATE_ERROR, // ERROR -> INACTIVE, DISCHARGE, ERROR +typedef enum { // states -> 3 bit. valid transitions: (all could transition to error) + STATE_INACTIVE, // INACTIVE -> PRECHARGE, CHARGING, ERROR + STATE_PRECHARGE, // PRECHARGE -> INACTIVE, READY, DISCHARGE, ERROR + STATE_READY, // READY -> ACTIVE, DISCHARGE, ERROR + STATE_ACTIVE, // ACTIVE -> READY, DISCHARGE, ERROR + STATE_DISCHARGE, // DISCHARGE -> INACTIVE, PRECHARGE, ERROR + STATE_CHARGING_PRECHARGE, + STATE_CHARGING, // CHARGING -> INACTIVE, DISCHARGE, ERROR + STATE_ERROR, // ERROR -> INACTIVE, DISCHARGE, ERROR } State; typedef struct { uint16_t bms_timeout : 1; - uint16_t bms_checksum_fail : 1; uint16_t bms_overtemp : 1; uint16_t bms_fault : 1; @@ -39,9 +49,9 @@ typedef struct { uint16_t temperature_sensor_missing : 1; uint16_t current_sensor_missing : 1; uint16_t voltage_missing : 1; + uint16_t battery_missing : 1; uint16_t relay_missing : 1; - uint16_t state_fail : 1; uint16_t state_transition_fail : 1; } ErrorKind; @@ -64,16 +74,17 @@ State sm_update_precharge(); State sm_update_ready(); State sm_update_active(); State sm_update_discharge(); +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_precharge_discharge(bool *is_closed, bool should_close); +void sm_charging_check(); +void sm_handle_ams_in(); void sm_check_errors(); +void sm_set_error(ErrorKind error_kind, bool is_errored); -void sm_handle_ams_in(const uint8_t *data); - -void sm_set_error(ErrorKind error_kind, bool is_errored); \ No newline at end of file +#endif /* "INC_STATE_MACHINE_H" */ \ No newline at end of file diff --git a/Core/Src/can.c b/Core/Src/can.c index 747ff35..ab1eadb 100644 --- a/Core/Src/can.c +++ b/Core/Src/can.c @@ -24,5 +24,5 @@ void can_handle_send_status() { } void can_handle_recieve_command(const uint8_t *data){ - ftcan_msg_received_cb(0x501, 8, data); + ftcan_msg_received_cb(0x501, 16, data); } diff --git a/Core/Src/state_machine.c b/Core/Src/state_machine.c index b79f3e0..2a2322a 100644 --- a/Core/Src/state_machine.c +++ b/Core/Src/state_machine.c @@ -1,74 +1,72 @@ #include "state_machine.h" -#include "ADBMS_LL_Driver.h" -#include "AMS_HighLevel.h" -#include "stm32f3xx_hal.h" -#include "ADBMS_Abstraction.h" -#include "main.h" StateHandle state; static bool relay_closed = 0; static bool precharge_closed = 0; -static int16_t RELAY_BAT_SIDE = 0; -static int16_t RELAY_ESC_SIDE = 0; -static int16_t CURRENT_MEASUREMENT = 0; +static int16_t RELAY_BAT_SIDE_VOLTAGE = 0; +static int16_t RELAY_ESC_SIDE_VOLTAGE = 0; +static int16_t CURRENT_MEASUREMENT_VOLTAGE = 0; +static int16_t timestamp; void sm_init(){ state.current_state = STATE_INACTIVE; state.target_state = STATE_INACTIVE; state.error_source = 0; + RELAY_BAT_SIDE_VOLTAGE = module.auxVoltages[0]; + RELAY_ESC_SIDE_VOLTAGE = module.auxVoltages[1]; + CURRENT_MEASUREMENT_VOLTAGE = module.auxVoltages[2]; } void sm_update(){ - + sm_handle_ams_in(); switch (state.current_state) { case STATE_INACTIVE: - state.current_state = sm_update_inactive(); // monitor only + 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 + 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 + 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 + 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 + 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. + 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 + state.current_state = sm_update_error(); // enter the correct ERROR state break; } sm_set_relay_positions(state.current_state); - //status_led_state(state.current_state, (ErrorKind) state.error_type); } State sm_update_inactive(){ switch (state.target_state) { case STATE_PRECHARGE: - //close precharge relay, wait until both sides are similar - sm_set_relay_positions(STATE_PRECHARGE); return STATE_PRECHARGE; - case STATE_CHARGING: - return STATE_CHARGING; - default: + case STATE_CHARGING_PRECHARGE: + return STATE_CHARGING_PRECHARGE; + default: return STATE_INACTIVE; } } State sm_update_precharge(){ switch (state.target_state) { - case STATE_INACTIVE: - return STATE_INACTIVE; - case STATE_READY: - return STATE_READY; - case STATE_DISCHARGE: + case STATE_INACTIVE: // if CAN Signal 0000 0000 then immidiete shutdown return STATE_DISCHARGE; + case STATE_READY: + if (RELAY_BAT_SIDE_VOLTAGE == RELAY_ESC_SIDE_VOLTAGE) + return STATE_READY; default: return STATE_PRECHARGE; } @@ -76,9 +74,9 @@ State sm_update_precharge(){ State sm_update_ready(){ switch (state.target_state) { - case STATE_ACTIVE: + case STATE_ACTIVE: // if CAN Signal 1100 0000 then turn on powerground return STATE_ACTIVE; - case STATE_DISCHARGE: + case STATE_DISCHARGE: // if CAN Signal 0000 0000 then shutdown return STATE_DISCHARGE; default: return STATE_READY; @@ -87,9 +85,9 @@ State sm_update_ready(){ State sm_update_active(){ switch (state.target_state) { - case STATE_READY: - return STATE_READY; - case STATE_DISCHARGE: + 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; @@ -99,18 +97,28 @@ State sm_update_active(){ State sm_update_discharge(){ switch (state.target_state) { case STATE_INACTIVE: - return STATE_INACTIVE; - case STATE_PRECHARGE: + if (RELAY_ESC_SIDE_VOLTAGE == 0) + return STATE_INACTIVE; + 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_INACTIVE: - return STATE_INACTIVE; case STATE_DISCHARGE: return STATE_DISCHARGE; default: @@ -118,10 +126,9 @@ State sm_update_charging(){ } } + State sm_update_error(){ switch (state.target_state) { - case STATE_INACTIVE: - return STATE_INACTIVE; case STATE_DISCHARGE: return STATE_DISCHARGE; default: @@ -148,7 +155,7 @@ void sm_set_relay_positions(State current_state){ sm_set_relay(RELAY_PRECHARGE, 0); case STATE_DISCHARGE: sm_set_relay(RELAY_MAIN, 0); - sm_set_relay(RELAY_PRECHARGE, 1); + sm_set_relay(RELAY_PRECHARGE, 0); break; case STATE_CHARGING: sm_set_relay(RELAY_MAIN, 1); @@ -156,6 +163,7 @@ void sm_set_relay_positions(State current_state){ break; case STATE_ERROR: sm_set_relay(RELAY_MAIN, 0); + sm_set_relay(RELAY_PRECHARGE, 0); break; } } @@ -174,14 +182,46 @@ void sm_set_relay(Relay relay, bool closed){ } } +void sm_handle_ams_in(){ + uint8_t data[2] = {}; + can_handle_recieve_command(&data); + switch (data[0]) { + case 0b00000000: + if (state.current_state != STATE_INACTIVE){ + PWM_powerground_control(0); + state.target_state = STATE_DISCHARGE; + } + break; + case 0b10000000: + if (state.target_state == STATE_INACTIVE || state.target_state == STATE_DISCHARGE){ + PWM_powerground_control(0); + state.target_state = STATE_PRECHARGE; + } else if (state.target_state == STATE_ACTIVE){ + PWM_powerground_control(0); + state.target_state = STATE_READY; + } + break; + case 0b11000000: + PWM_powerground_control(data[1]); + state.target_state = STATE_ACTIVE; // READY -> ACTIVE + break; + } +} -void sm_check_precharge_discharge(bool *is_closed, bool should_close){} - // compare RELAY_BATT_SIDE and RELAY_ESC_SIDE - // if (state.current_state == STATE_PRECHARGE && (RELAY_ESC_SIDE < RELAY_BAT_SIDE)) //-> don't switch from PRECHARGE to READY - // if (state.current_state == STATE_DISCHARGE && (RELAY_ESC_SIDE > 12V)) -> don't switch from DISCHARGE to INACTIVE +void sm_set_error(ErrorKind error_kind, bool is_errored){} -void sm_handle_ams_in(const uint8_t *data){} +void sm_check_errors(){ + if (module.status.THSD == 1) { + state.error_type.bms_overtemp = 1; + } + if (RELAY_BAT_SIDE_VOLTAGE < 40){ + state.error_source = (1 << 10); + } +} -void sm_set_error(ErrorKind error_kind, bool is_errored); - -void sm_check_errors(){} \ No newline at end of file +void sm_charging_check(){ + 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; +} \ No newline at end of file diff --git a/Documentation/State machine diagram.drawio b/Documentation/State machine diagram.drawio index 822ad37..69a3678 100644 --- a/Documentation/State machine diagram.drawio +++ b/Documentation/State machine diagram.drawio @@ -1,15 +1,15 @@ - + - + - + - + @@ -20,7 +20,7 @@ - + @@ -30,14 +30,14 @@ - + - + - - + + @@ -49,7 +49,7 @@ - + @@ -59,13 +59,13 @@ - + - - + + @@ -76,21 +76,20 @@ - + - - - + + - + - + @@ -98,8 +97,8 @@ - - + + @@ -109,10 +108,17 @@ - + - - + + + + + + + + + @@ -122,8 +128,8 @@ - - + + @@ -133,7 +139,7 @@ - + @@ -143,12 +149,12 @@ - - + + - + @@ -157,13 +163,29 @@ - + - + + + + + + + + + + + + + + + + + diff --git a/Documentation/State machine diagram.svg b/Documentation/State machine diagram.svg index 2e3cfbb..58f8d46 100644 --- a/Documentation/State machine diagram.svg +++ b/Documentation/State machine diagram.svg @@ -1,4 +1,4 @@ -CAN Signal: 1000 0000RELAY_BAT_SIDE > RELAY_ESC_SIDEfor longer than 5 seconds
INACTIVE
INACTIVEAllows quick shutdown. Could be done with ERRORRELAY_BAT_SIDE = RELAY_ESC_SIDEif it is around 90% done
PRECHARGE
PRECHARGEAllows quick reactivation instead of shutdownRELAY_ESC_SIDE == 0
DISCHARGE
DISCHARGECAN Signal = 1100 0000
READY
READYCAN Signal: 0000 0000 ORBattery out of charge
CAN_Signal: 1000 0000
CAN_Signal: 1000 0000
ACTIVE
ACTIVEPRECHARGE is done
CHARGING
PRECHAGE
CHARGING...CHARGING is done
CHARGING
CHARGINGCAN_Signal: 0000 0000 Text is not SVG - cannot display \ No newline at end of file +CAN Signal: 1000 0000RELAY_BAT_SIDE > RELAY_ESC_SIDEfor longer than 5 seconds
INACTIVE
INACTIVEAllows quick shutdown. Could be done with ERRORRELAY_BAT_SIDE = RELAY_ESC_SIDEif it is around 90% done
PRECHARGE
PRECHARGEAllows quick reactivation instead of shutdownRELAY_ESC_SIDE == 0
DISCHARGE
DISCHARGECAN Signal = 1100 0000
READY
READYCAN Signal: 0000 0000 ORBattery out of charge
CAN_Signal: 1000 0000
CAN_Signal: 1000 0000
ACTIVE
ACTIVEPRECHARGE is done
CHARGING
PRECHAGE
CHARGING...CHARGING is done
CHARGING
CHARGINGCAN_Signal: 0000 0000
PRECHARGE failure
PRECHARGE failureText is not SVG - cannot display \ No newline at end of file