FaSTTUBe/steering-wheel
51
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update for new CAN dbc (incomplete), add more NamedFields, allow mission select without confirmation, set default mission select state to manual

Browse Source
This commit is contained in:
Leonard Gies
2025-07-08 13:17:33 +02:00
parent e995c0ad66
commit 91b62c7b5e
23 changed files with 1029 additions and 1349 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Core/Inc/stw_defines.h
View File

@ -6,11 +6,11 @@ typedef enum {
MISSION_NONE = 0,
MISSION_ACCEL = 1,
MISSION_SKIDPAD = 2,
MISSION_AUTOX = 3,
MISSION_TRACKDRIVE = 4,
MISSION_EBS = 5,
MISSION_INSPECTION = 6,
MISSION_MANUAL = 7
MISSION_TRACKDRIVE = 3,
MISSION_BRAKETEST = 4,
MISSION_INSPECTION = 5,
MISSION_AUTOX = 6,
MISSION_MANUAL = 7,
} Mission;
#endif // __INC_STW_DEFINES_H

211
Core/Inc/vehicle_state.h
View File

@ -7,10 +7,9 @@ extern "C" {
#include "params.h"
#include "stw_defines.h"
#include <stdbool.h>
#include <stdint.h>
#define NUM_CONES 12
typedef enum {
TS_INACTIVE = 0,
TS_ACTIVE = 1,
@ -18,7 +17,7 @@ typedef enum {
TS_DISCHARGE = 3,
TS_ERROR = 4,
TS_CHARGING_CHECK = 5,
TS_CHARGING = 6
TS_CHARGING = 6,
} TSState;
typedef enum {
@ -27,41 +26,51 @@ typedef enum {
AS_READY = 2,
AS_DRIVING = 3,
AS_FINISHED = 4,
AS_EMERGENCY = 5
AS_EMERGENCY = 5,
} ASState;
typedef enum {
R2D_NONE = 0,
R2D_TSMS = 1,
R2D_SDC_CLOSED = 1,
R2D_TSACTIVE = 2,
R2D_RESETTING_NODES = 3,
R2D_RESETTING_COMMS = 4,
R2D_WAITING_INIT = 5,
R2D_INIT_STAGE1 = 6,
R2D_INIT_STAGE2 = 7,
R2D_INIT_SUCCESS = 0xF
R2D_INIT_SUCCESS = 0xF,
} R2DProgress;
typedef enum {
SDC_OFF = 0,
SDC_PDU_OK = 1, //
SDC_RES_OK = 2,
SDC_AMS_OK = 3,
SDC_IMD_OK = 4,
SDC_BOTS_OK = 5,
SDC_SDB_DB_OK = 6, // shut down button dashboard
SDC_IS_OK = 7,
SDC_BSPD_OK = 8,
SDC_SDB_L_OK = 9,
SDC_SDB_R_OK = 10,
SDC_TSMS_OK = 11,
SDC_HVD_OK = 12,
} SDCStatus;
typedef enum {
INICHK_START = 0,
INICHK_WD_CHECK = 1,
INICHK_WD_OK = 2,
INICHK_POS_CHK_1 = 3,
INICHK_ASB_CHECK_A_1 = 4,
INICHK_ASB_CHECK_A_2 = 5,
INICHK_SWITCH_B = 6,
INICHK_ASB_CHECK_B_1 = 7,
INICHK_ASB_CHECK_B_2 = 8,
INICHK_SWITCH_A = 9,
INICHK_ASB_CHECK_AB_1 = 10,
INICHK_ASB_CHECK_AB_2 = 11,
INICHK_POS_CHK_2 = 12,
INICHK_WAIT_TS = 13,
INICHK_DONE = 14,
INICHK_ERROR = 15
} IniChkState;
INICHK_ASB_CHECK_1 = 3,
INICHK_ASB_CHECK_2 = 4,
INICHK_WAIT_TS = 5,
INICHK_EBS_CHECK_A = 6,
INICHK_EBS_CHECK_B = 7,
INICHK_DONE = 8,
INICHK_ERROR = 9,
} InitialCheckupState;
const char *inichkstate_str(IniChkState state);
const char *inichkstate_str(InitialCheckupState state);
typedef enum {
AMS_ERROR_NONE = 0x00,
@ -69,7 +78,7 @@ typedef enum {
AMS_ERROR_SLAVE_PANIC = 0x02,
AMS_ERROR_SHUNT_TIMEOUT = 0x03,
AMS_ERROR_SHUNT_OVERCURRENT = 0x04,
AMS_ERROR_SHUNT_OVERTEMP = 0x05
AMS_ERROR_SHUNT_OVERTEMP = 0x05,
} AMSErrorKind;
typedef enum {
AMS_SLAVEPANIC_OVERTEMP = 0x00,
@ -77,9 +86,21 @@ typedef enum {
AMS_SLAVEPANIC_OVERVOLTAGE = 0x02,
AMS_SLAVEPANIC_UNDERVOLTAGE = 0x03,
AMS_SLAVEPANIC_TOO_FEW_TEMP = 0x04,
AMS_SLAVEPANIC_OPENWIRE = 0x05
AMS_SLAVEPANIC_OPENWIRE = 0x05,
} AMSSlavePanicKind;
typedef struct {
AMSErrorKind kind;
uint8_t arg;
} AMSError;
typedef struct {
float wss_fl; // in km/h
float wss_fr; // in km/h
float wss_rl; // in km/h
float wss_rr; // in km/h
} WheelSpeeds;
typedef struct {
float tire_fl;
float tire_fr;
@ -91,12 +112,10 @@ typedef struct {
float brake_rl;
float brake_rr;
float inv_l;
float inv_r;
float mot_l;
float mot_r;
float bat_l;
float bat_r;
float inv_1;
float inv_2;
float mot_1;
float mot_2;
} Temperatures;
typedef struct {
@ -105,82 +124,96 @@ typedef struct {
} ConePosition;
typedef struct {
TSState ts_state;
ASState as_state;
Mission active_mission;
int sdc_closed;
int pdu_sdc_active;
int imd_ok;
int sdcl_state[3];
R2DProgress r2d_progress;
struct {
int invl_ready : 1;
int invr_ready : 1;
int sdc_bfl : 1;
int sdc_brl : 1;
int sdc_acc : 1;
int sdc_hvb : 1;
int err_sdc : 1;
int err_ams : 1;
int err_pdu : 1;
int err_ini_chk : 1;
int err_con_mon : 1;
int err_scs : 1;
int err_sbspd : 1;
int err_appsp : 1;
int err_as : 1;
int err_ros : 1;
int err_res : 1;
int err_invl : 1;
int err_invr : 1;
} errors;
struct {
AMSErrorKind kind;
uint8_t arg;
} last_ams_error;
// AMS / TS system
struct {
uint8_t id;
AMSSlavePanicKind kind;
uint32_t arg;
} last_ams_slave_panic;
IniChkState ini_chk_state;
TSState ts_state;
bool sdc_closed;
uint8_t soc_ts;
float min_cell_volt; // in V
float max_cell_temp; // in °C
bool imd_ok; // latest IMD measurement
bool tsal_green;
bool imd_error; // latched imd error
bool ams_error; // latched ams error
unsigned lap_count;
float lap_last;
float lap_best;
AMSError last_ams_error;
// dcdc temp not in temps because temps only contains temperatures with multiple instances
float dcdc_temp; // in °C
float dcdc_current; // in A
float ts_current; // in A
float ts_voltage_bat; // in V
float ts_voltage_veh; // in V
float shunt_temperature; // in °C
Temperatures temps;
float min_cell_volt;
float max_cell_temp;
int soc_ts;
float lap_time_best; // in s
float lap_time_last; // in s
float sector_time_best; // in s
float sector_time_last; // in s
float lv_bat_voltage;
int soc_lv;
WheelSpeeds wheel_speeds; // all in km/h
uint16_t distance_session; // in m
float ts_current;
float ts_voltage_bat;
float ts_voltage_veh;
float tank_pressure_1;
float tank_pressure_2;
float speed;
uint8_t apps_percent;
float brake_pressure_f;
float brake_pressure_r;
int8_t steering_angle;
float speed; // in m/s
uint8_t lap_count;
uint8_t sector_count;
float brake_press_f;
float brake_press_r;
float hyd_press_a;
float hyd_press_b;
R2DProgress r2d_progress;
struct {
bool err_pdu : 1;
bool err_res : 1;
bool err_as : 1;
bool err_apps_plausible : 1;
bool err_soft_bspd : 1;
bool err_scs : 1; // system critical signals
bool err_con_mon : 1; // continuous monitoring of autonomous system
bool err_initial_checkup : 1; // initial checkup
bool err_inv_1 : 1;
bool err_inv_2 : 1;
bool err_ams : 1;
bool err_sdc : 1; // sdc err = not closed
} errors;
SDCStatus sdc_status;
bool inv_1_ready;
bool inv_2_ready;
float energy_per_lap;
InitialCheckupState initial_checkup_state;
float distance_total;
// autonomous system
Mission active_mission;
ASState as_state;
uint32_t last_jetson_msg;
uint32_t last_epsc_msg;
int as_ok;
float desired_angle;
float measured_angle;
float desired_speed;
float inv_velocity_1;
float inv_velocity_2;
ConePosition cone_pos[NUM_CONES];
int16_t inv_torque_demanded_1;
int16_t inv_torque_demanded_2;
int16_t inv_torque_desired_1;
int16_t inv_torque_desired_2;
uint16_t inv_control_word_1;
uint16_t inv_control_word_2;
int16_t inv_torque_actual_1;
int16_t inv_torque_actual_2;
uint16_t inv_errors_1;
uint16_t inv_errors_2;
uint16_t inv_warnings_1;
uint16_t inv_warnings_2;
ParamType last_param_confirmed;

464
Core/Src/vehicle.c
View File

@ -10,72 +10,102 @@
#include "tx_api.h"
#include "ui.h"
#include "vehicle_state.h"
#include <stdbool.h>
#define CAN_ID_AMS_SLAVE_PANIC 0x9
#define CAN_ID_AMS_STATUS 0xA
#define CAN_ID_AMS_ERROR 0xC
#define CAN_ID_SDCL_TX 0x10
#define CAN_ID_PDU_RESPONSE 0xC9
#define CAN_ID_JETSON_TX 0xE1
#define CAN_ID_ABX_DRIVER 0x101
#define CAN_ID_ABX_TIMINGS 0x102
#define CAN_ID_ABX_BRAKE_T 0x105
#define CAN_ID_CS_INTERNAL 0x108
#define CAN_ID_ABX_MISC 0x109
#define CAN_ID_ABX_HYDRAULICS 0x110
#define CAN_ID_EPSC_OUT 0x321
#define CAN_ID_MISSION_SELECTED 0x400
#define CAN_ID_STW_BUTTONS 0x401
#define CAN_ID_STW_PARAM_SET 0x402
// CAN filter constants
#define CAN_ID_AMS_SLAVE_PANIC 0x009
#define CAN_ID_AMS_STATUS 0x00A
#define CAN_ID_AMS_ERROR 0x00C
#define CAN_ID_AMS_DCDC 0x313
#define CAN_ID_PWM_DUTYCYLE 0x0DC
#define CAN_ID_PWM_CONFIG 0x0DD
#define CAN_ID_FTCU_TIMINGS 0x102
#define CAN_ID_FTCU_DAMPER 0x103
#define CAN_ID_FTCU_WHEELSPEED 0x104
#define CAN_ID_FTCU_BRAKE_T 0x105
#define CAN_ID_FTCU_COOLING 0x107
#define CAN_ID_FTCU_PNEUMATIK 0x110
#define CAN_ID_FTCU_DRIVER 0x111
#define CAN_ID_AS_MISSION_FB 0x410
#define CAN_ID_STW_STATUS 0x412
#define CAN_ID_ABX_PARAM_CONFIRMED 0x413
#define CAN_ID_STW_CONES_BASE 0x414
#define CAN_ID_STW_CONES_MASK 0x7FC
#define CAN_ID_FTCU_PARAM_CONFIRMED 0x413
#define CAN_ID_FTCU_TELEMETRIE1 0x719
#define CAN_ID_FTCU_TELEMETRIE2 0x720
#define CAN_ID_PDU_RESPONSE 0x0C9
#define CAN_ID_PDU_CURRENT1 0x0CA
#define CAN_ID_PDU_CURRENT2 0x0CB
#define CAN_ID_PDU_CURRENT3 0x0CC
#define CAN_ID_PDU_CURRENT4 0x0CD
#define CAN_ID_DASHBOARD 0x420
#define CAN_ID_SSU 0x500
#define CAN_ID_SHUNT_CURRENT 0x521
#define CAN_ID_SHUNT_VOLTAGE1 0x522
#define CAN_ID_SHUNT_VOLTAGE2 0x523
#define CAN_AMS_STATUS_VOLTAGE_FACTOR 1e-4
#define CAN_AMS_STATUS_TEMP_FACTOR 0.0625
#define CAN_SDCL_STATE_1_MASK (1 << 1)
#define CAN_SDCL_STATE_2_MASK (1 << 2)
#define CAN_SDCL_STATE_3_MASK (1 << 3)
#define CAN_PDU_RESPONSE_SDC_TX_MASK (1 << 4)
#define CAN_JETSON_TX_AS_OK_MASK (1 << 1)
#define CAN_JETSON_TX_ANGLE_FACTOR 0.00784314f
#define CAN_JETSON_TX_SPEED_FACTOR (0.2 * 3.6)
#define CAN_ABX_DRIVER_SPEED_FACTOR (0.2 * 3.6)
#define CAN_ABX_DRIVER_PRESS_FACTOR 0.1f
#define CAN_CS_INTERNAL_TEMP_FACTOR 0.01
#define CAN_ABX_MISC_DISTANCE_TOTAL_FACTOR 0.01
#define CAN_ABX_MISC_LV_BAT_VOLTAGE_FACTOR (15.0f / 255)
#define CAN_EPSC_OUT_ANGLE_FACTOR 7.20721e-05f
#define CAN_ID_SHUNT_TEMPERATURE 0x525
#define CAN_ID_TTS_FL 0x701
#define CAN_ID_TTS_FR 0x702
#define CAN_ID_TTS_RL 0x703
#define CAN_ID_TTS_RR 0x704
#define CAN_ID_INVERTER_VELOCITY 0x776
#define CAN_ID_INVERTER_TORQUE_WANTED 0x777
#define CAN_ID_INVERTER_TEMPERATURE 0x778
#define CAN_ID_INVERTER_TORQUE_ACTUAL 0x779
#define CAN_ID_INVERTER_ERRORS_WARNINGS 0x780
// CAN sending constants
#define CAN_ID_MISSION_SELECTED 0x400
#define CAN_ID_STW_BUTTONS 0x401
#define CAN_ID_STW_PARAM_SET 0x402
void vehicle_thread_entry(ULONG hfdcan_addr) {
memset(&vehicle_state, 0, sizeof(vehicle_state));
memset(&vehicle_state.cone_pos, 0xFF, sizeof(vehicle_state.cone_pos));
ftcan_init((void *)hfdcan_addr);
ftcan_add_filter(CAN_ID_AMS_SLAVE_PANIC, 0x7FF);
ftcan_add_filter(CAN_ID_AMS_STATUS, 0x7FF);
ftcan_add_filter(CAN_ID_AMS_ERROR, 0x7FF);
ftcan_add_filter(CAN_ID_SDCL_TX, 0x7FF);
ftcan_add_filter(CAN_ID_PDU_RESPONSE, 0x7FF);
ftcan_add_filter(CAN_ID_JETSON_TX, 0x7FF);
ftcan_add_filter(CAN_ID_ABX_DRIVER, 0x7FF);
ftcan_add_filter(CAN_ID_ABX_TIMINGS, 0x7FF);
ftcan_add_filter(CAN_ID_ABX_BRAKE_T, 0x7FF);
ftcan_add_filter(CAN_ID_CS_INTERNAL, 0x7FF);
ftcan_add_filter(CAN_ID_ABX_MISC, 0x7FF);
ftcan_add_filter(CAN_ID_ABX_HYDRAULICS, 0x7FF);
ftcan_add_filter(CAN_ID_EPSC_OUT, 0x7FF);
ftcan_add_filter(CAN_ID_AMS_DCDC, 0x7FF);
ftcan_add_filter(CAN_ID_PWM_DUTYCYLE, 0x7FF);
ftcan_add_filter(CAN_ID_PWM_CONFIG, 0x7FF);
ftcan_add_filter(CAN_ID_FTCU_TIMINGS, 0x7FF);
ftcan_add_filter(CAN_ID_FTCU_DAMPER, 0x7FF);
ftcan_add_filter(CAN_ID_FTCU_WHEELSPEED, 0x7FF);
ftcan_add_filter(CAN_ID_FTCU_BRAKE_T, 0x7FF);
ftcan_add_filter(CAN_ID_FTCU_COOLING, 0x7FF);
ftcan_add_filter(CAN_ID_FTCU_PNEUMATIK, 0x7FF);
ftcan_add_filter(CAN_ID_FTCU_DRIVER, 0x7FF);
ftcan_add_filter(CAN_ID_AS_MISSION_FB, 0x7FF);
ftcan_add_filter(CAN_ID_STW_STATUS, 0x7FF);
ftcan_add_filter(CAN_ID_ABX_PARAM_CONFIRMED, 0x7FF);
ftcan_add_filter(CAN_ID_STW_CONES_BASE, CAN_ID_STW_CONES_MASK);
ftcan_add_filter(CAN_ID_FTCU_PARAM_CONFIRMED, 0x7FF);
ftcan_add_filter(CAN_ID_FTCU_TELEMETRIE1, 0x7FF);
ftcan_add_filter(CAN_ID_FTCU_TELEMETRIE2, 0x7FF);
ftcan_add_filter(CAN_ID_PDU_RESPONSE, 0x7FF);
ftcan_add_filter(CAN_ID_PDU_CURRENT1, 0x7FF);
ftcan_add_filter(CAN_ID_PDU_CURRENT2, 0x7FF);
ftcan_add_filter(CAN_ID_PDU_CURRENT3, 0x7FF);
ftcan_add_filter(CAN_ID_PDU_CURRENT4, 0x7FF);
ftcan_add_filter(CAN_ID_DASHBOARD, 0x7FF);
ftcan_add_filter(CAN_ID_SSU, 0x7FF);
ftcan_add_filter(CAN_ID_SHUNT_CURRENT, 0x7FF);
ftcan_add_filter(CAN_ID_SHUNT_VOLTAGE1, 0x7FF);
ftcan_add_filter(CAN_ID_SHUNT_VOLTAGE2, 0x7FF);
ftcan_add_filter(CAN_ID_SHUNT_TEMPERATURE, 0x7FF);
ftcan_add_filter(CAN_ID_TTS_FL, 0x7FF);
ftcan_add_filter(CAN_ID_TTS_FR, 0x7FF);
ftcan_add_filter(CAN_ID_TTS_RL, 0x7FF);
ftcan_add_filter(CAN_ID_TTS_RL, 0x7FF);
ftcan_add_filter(CAN_ID_INVERTER_VELOCITY, 0x7FF);
ftcan_add_filter(CAN_ID_INVERTER_TORQUE_WANTED, 0x7FF);
ftcan_add_filter(CAN_ID_INVERTER_TEMPERATURE, 0x7FF);
ftcan_add_filter(CAN_ID_INVERTER_TORQUE_ACTUAL, 0x7FF);
ftcan_add_filter(CAN_ID_INVERTER_ERRORS_WARNINGS, 0x7FF);
while (1) {
tx_thread_sleep(10);
@ -83,8 +113,7 @@ void vehicle_thread_entry(ULONG hfdcan_addr) {
#ifdef DEMO_MODE
double tick = HAL_GetTick();
vehicle_state.speed =
(sin(tick * 0.001) * 8 + 10 + cos(tick * 0.003) * 8) * 4;
vehicle_state.speed = (sin(tick * 0.001) * 8 + 10 + cos(tick * 0.003) * 8) * 4;
if (vehicle_state.speed <= 0) {
vehicle_state.speed = 0;
}
@ -107,160 +136,199 @@ void vehicle_broadcast_param(ParamType param, int32_t value) {
}
void vehicle_broadcast_buttons(GPIO_PinState *button_states) {
uint8_t data = (button_states[DRS_BUTTON_IDX] << 0) |
(button_states[6] << 1) | (button_states[7] << 2);
uint8_t data = (button_states[DRS_BUTTON_IDX] << 0) | (button_states[6] << 1) | (button_states[7] << 2);
ftcan_transmit(CAN_ID_STW_BUTTONS, &data, 1);
}
void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
const uint8_t *ptr;
if ((id & CAN_ID_STW_CONES_MASK) == CAN_ID_STW_CONES_BASE) {
size_t msg_num = id - CAN_ID_STW_CONES_BASE;
for (size_t i = 0; i < 4; i++) {
vehicle_state.cone_pos[msg_num * 4 + i].x = data[i * 2];
vehicle_state.cone_pos[msg_num * 4 + i].y = data[i * 2 + 1];
}
} else {
switch (id) {
case CAN_ID_AMS_SLAVE_PANIC:
vehicle_state.last_ams_slave_panic.id = data[0];
vehicle_state.last_ams_slave_panic.kind = data[1];
ptr = &data[2];
vehicle_state.last_ams_slave_panic.arg =
ftcan_unmarshal_unsigned(&ptr, 4);
break;
case CAN_ID_AMS_STATUS:
vehicle_state.ts_state = data[0] & 0x7F;
vehicle_state.sdc_closed = (data[0] & 0x80) >> 7;
vehicle_state.soc_ts = data[1];
ptr = &data[2];
vehicle_state.min_cell_volt =
ftcan_unmarshal_unsigned(&ptr, 2) * CAN_AMS_STATUS_VOLTAGE_FACTOR;
vehicle_state.max_cell_temp =
ftcan_unmarshal_signed(&ptr, 2) * CAN_AMS_STATUS_TEMP_FACTOR;
vehicle_state.imd_ok = (data[6] >> 7);
// TODO: Separate temperatures for left and right side of battery
vehicle_state.temps.bat_l = vehicle_state.max_cell_temp;
vehicle_state.temps.bat_r = vehicle_state.max_cell_temp;
break;
case CAN_ID_SDCL_TX:
vehicle_state.sdcl_state[0] = data[0] & CAN_SDCL_STATE_1_MASK;
vehicle_state.sdcl_state[1] = data[0] & CAN_SDCL_STATE_2_MASK;
vehicle_state.sdcl_state[2] = data[0] & CAN_SDCL_STATE_3_MASK;
break;
case CAN_ID_PDU_RESPONSE:
vehicle_state.pdu_sdc_active = data[0] & CAN_PDU_RESPONSE_SDC_TX_MASK;
break;
case CAN_ID_AMS_ERROR:
vehicle_state.last_ams_error.kind = data[0];
vehicle_state.last_ams_error.arg = data[1];
break;
case CAN_ID_JETSON_TX:
vehicle_state.last_jetson_msg = HAL_GetTick();
vehicle_state.as_ok = data[0] & CAN_JETSON_TX_AS_OK_MASK;
vehicle_state.desired_speed =
((int8_t)data[1]) * CAN_JETSON_TX_SPEED_FACTOR;
vehicle_state.desired_angle =
((int8_t)data[2]) * CAN_JETSON_TX_ANGLE_FACTOR;
break;
case CAN_ID_ABX_DRIVER:
vehicle_state.brake_press_f =
(data[1] | ((data[2] & 0x0F) << 8)) * CAN_ABX_DRIVER_PRESS_FACTOR;
vehicle_state.brake_press_r =
((data[2] >> 4) | (data[3] << 4)) * CAN_ABX_DRIVER_PRESS_FACTOR;
vehicle_state.speed = data[5] * CAN_ABX_DRIVER_SPEED_FACTOR;
break;
case CAN_ID_ABX_HYDRAULICS:
vehicle_state.hyd_press_a =
(data[0] | ((data[1] & 0x0F) << 8)) * CAN_ABX_DRIVER_PRESS_FACTOR;
vehicle_state.hyd_press_b =
((data[1] >> 4) | (data[2] << 4)) * CAN_ABX_DRIVER_PRESS_FACTOR;
break;
case CAN_ID_ABX_TIMINGS:
vehicle_state.lap_best = (data[0] | (data[1] << 8)) * 0.01f;
vehicle_state.lap_last = (data[2] | (data[3] << 8)) * 0.01f;
break;
case CAN_ID_ABX_BRAKE_T:
vehicle_state.temps.brake_fl = (data[0] | (data[1] << 8)) * 0.01f;
vehicle_state.temps.brake_fr = (data[2] | (data[3] << 8)) * 0.01f;
vehicle_state.temps.brake_rl = (data[4] | (data[5] << 8)) * 0.01f;
vehicle_state.temps.brake_rr = (data[6] | (data[7] << 8)) * 0.01f;
break;
case CAN_ID_CS_INTERNAL:
vehicle_state.temps.inv_l =
(data[0] | (data[1] << 8)) * CAN_CS_INTERNAL_TEMP_FACTOR;
vehicle_state.temps.inv_r =
(data[2] | (data[3] << 8)) * CAN_CS_INTERNAL_TEMP_FACTOR;
vehicle_state.temps.mot_l =
(data[4] | (data[5] << 8)) * CAN_CS_INTERNAL_TEMP_FACTOR;
vehicle_state.temps.mot_r =
(data[6] | (data[7] << 8)) * CAN_CS_INTERNAL_TEMP_FACTOR;
break;
case CAN_ID_ABX_MISC:
vehicle_state.distance_total =
(data[3] | (data[4] << 8)) * CAN_ABX_MISC_DISTANCE_TOTAL_FACTOR;
vehicle_state.soc_lv = data[5];
vehicle_state.lv_bat_voltage =
data[6] * CAN_ABX_MISC_LV_BAT_VOLTAGE_FACTOR;
break;
case CAN_ID_EPSC_OUT:
vehicle_state.last_epsc_msg = HAL_GetTick();
vehicle_state.measured_angle =
((int16_t)((data[0] << 8) | (data[1]))) * CAN_EPSC_OUT_ANGLE_FACTOR;
break;
case CAN_ID_AS_MISSION_FB:
vehicle_state.active_mission = data[0] & 0b111;
break;
case CAN_ID_STW_STATUS:
vehicle_state.as_state = data[0] & 0b111;
vehicle_state.r2d_progress = data[0] >> 4;
vehicle_state.errors.invl_ready = (data[1] >> 0) & 1;
vehicle_state.errors.invr_ready = (data[1] >> 1) & 1;
vehicle_state.errors.sdc_bfl = (data[1] >> 2) & 1;
vehicle_state.errors.sdc_brl = (data[1] >> 3) & 1;
vehicle_state.errors.sdc_acc = (data[1] >> 4) & 1;
vehicle_state.errors.sdc_hvb = (data[1] >> 5) & 1;
vehicle_state.lap_count = data[2] & 0b111111;
vehicle_state.ini_chk_state = data[3];
vehicle_state.errors.err_sdc = (data[4] >> 0) & 1;
vehicle_state.errors.err_ams = (data[4] >> 1) & 1;
vehicle_state.errors.err_pdu = (data[4] >> 2) & 1;
vehicle_state.errors.err_ini_chk = (data[4] >> 3) & 1;
vehicle_state.errors.err_con_mon = (data[4] >> 4) & 1;
vehicle_state.errors.err_scs = (data[4] >> 5) & 1;
vehicle_state.errors.err_sbspd = (data[4] >> 6) & 1;
vehicle_state.errors.err_appsp = (data[4] >> 7) & 1;
vehicle_state.errors.err_as = (data[5] >> 0) & 1;
vehicle_state.errors.err_ros = (data[5] >> 1) & 1;
vehicle_state.errors.err_res = (data[5] >> 2) & 1;
vehicle_state.errors.err_invl = (data[5] >> 3) & 1;
vehicle_state.errors.err_invr = (data[5] >> 4) & 1;
break;
case CAN_ID_ABX_PARAM_CONFIRMED:
vehicle_state.last_param_confirmed = data[0];
tx_event_flags_set(&gui_update_events, GUI_UPDATE_PARAM_CONFIRMED,
TX_OR);
break;
case CAN_ID_SHUNT_CURRENT: {
// The first two bytes of shunt result messages are metadata
const uint8_t *result_ptr = &data[2];
vehicle_state.ts_current =
ftcan_unmarshal_signed(&result_ptr, 4) * 1e-3;
break;
}
case CAN_ID_SHUNT_VOLTAGE1: {
const uint8_t *result_ptr = &data[2];
vehicle_state.ts_voltage_bat =
ftcan_unmarshal_signed(&result_ptr, 4) * 1e-3;
break;
}
case CAN_ID_SHUNT_VOLTAGE2: {
const uint8_t *result_ptr = &data[2];
vehicle_state.ts_voltage_veh =
ftcan_unmarshal_signed(&result_ptr, 4) * 1e-3;
break;
}
}
switch (id) {
case CAN_ID_AMS_SLAVE_PANIC:
vehicle_state.last_ams_slave_panic.id = data[0];
vehicle_state.last_ams_slave_panic.kind = data[1];
ptr = &data[2];
vehicle_state.last_ams_slave_panic.arg = ftcan_unmarshal_unsigned(&ptr, 4);
break;
case CAN_ID_AMS_STATUS:
vehicle_state.ts_state = data[0] & 0x7F;
vehicle_state.sdc_closed = (data[0] & 0x80) >> 7;
vehicle_state.soc_ts = data[1];
ptr = &data[2];
vehicle_state.min_cell_volt = ftcan_unmarshal_unsigned(&ptr, 2) * 1e-3;
vehicle_state.max_cell_temp = ftcan_unmarshal_signed(&ptr, 2) * 1e-2;
// vehicle_state.imd_state = data[6] & 0x7F; // does not provide useful
// data
vehicle_state.imd_ok = (data[6] & 0x80) >> 7;
vehicle_state.tsal_green = data[7] & 0x01;
vehicle_state.imd_error = (data[7] & 0x02) >> 1;
vehicle_state.ams_error = (data[7] & 0x04) >> 2;
break;
case CAN_ID_AMS_ERROR:
vehicle_state.last_ams_error.kind = data[0];
vehicle_state.last_ams_error.arg = data[1];
break;
case CAN_ID_AMS_DCDC:
ptr = &data[0];
vehicle_state.dcdc_temp = ftcan_unmarshal_signed(&ptr, 2) * 0.01f;
vehicle_state.dcdc_current = ftcan_unmarshal_signed(&ptr, 2) * 0.001f;
break;
case CAN_ID_PWM_DUTYCYLE:
// TODO
break;
case CAN_ID_PWM_CONFIG:
// TODO
break;
case CAN_ID_FTCU_TIMINGS:
vehicle_state.lap_time_best = (data[0] | (data[1] << 8)) * 0.01f;
vehicle_state.lap_time_last = (data[2] | (data[3] << 8)) * 0.01f;
vehicle_state.sector_time_best = (data[4] | (data[5] << 8)) * 0.01f;
vehicle_state.sector_time_last = (data[6] | (data[7] << 8)) * 0.01f;
break;
case CAN_ID_FTCU_DAMPER:
// TODO
break;
case CAN_ID_FTCU_WHEELSPEED:
vehicle_state.wheel_speeds.wss_fl = data[0] | ((data[1] & 0x0F) << 8);
vehicle_state.wheel_speeds.wss_fr = ((data[1] >> 4) & 0x0F) | (data[2] << 4);
vehicle_state.wheel_speeds.wss_rl = data[3] | ((data[4] & 0x0F) << 8);
vehicle_state.wheel_speeds.wss_rr = ((data[4] >> 4) & 0x0F) | (data[5] << 4);
vehicle_state.distance_session = data[6] | (data[7] << 8);
break;
case CAN_ID_FTCU_BRAKE_T:
vehicle_state.temps.brake_fl = (data[0] | (data[1] << 8)) * 0.01f;
vehicle_state.temps.brake_fr = (data[2] | (data[3] << 8)) * 0.01f;
vehicle_state.temps.brake_rl = (data[4] | (data[5] << 8)) * 0.01f;
vehicle_state.temps.brake_rr = (data[6] | (data[7] << 8)) * 0.01f;
break;
case CAN_ID_FTCU_COOLING:
// TODO
break;
case CAN_ID_FTCU_PNEUMATIK:
vehicle_state.tank_pressure_1 = data[0] | ((data[1] & 0x0F) << 8);
vehicle_state.tank_pressure_2 = data[2] | ((data[3] & 0x0F) << 8);
break;
case CAN_ID_FTCU_DRIVER:
vehicle_state.apps_percent = data[0];
vehicle_state.brake_pressure_f = (data[1] | ((data[2] & 0x0F) << 8)) * 0.1f;
vehicle_state.brake_pressure_r = (((data[2] >> 4) & 0x0F) | (data[3] << 4)) * 0.1f;
vehicle_state.steering_angle = data[4];
vehicle_state.speed = data[5] * 0.2f;
vehicle_state.lap_count = data[6];
vehicle_state.sector_count = data[7];
break;
case CAN_ID_AS_MISSION_FB:
vehicle_state.active_mission = data[0] & 0b111;
break;
case CAN_ID_STW_STATUS:
// vehicle_state.lap_count = data[0] & 0b111111;
vehicle_state.errors.err_pdu = (data[0] >> 6) & 0b1;
vehicle_state.errors.err_res = (data[0] >> 7) & 0b1;
vehicle_state.r2d_progress = (data[1] >> 0) & 0b1111;
vehicle_state.as_state = (data[1] >> 4) & 0b111;
vehicle_state.errors.err_as = (data[1] >> 7) & 0b1;
vehicle_state.errors.err_apps_plausible = (data[2] >> 0) & 0b1;
vehicle_state.errors.err_soft_bspd = (data[2] >> 1) & 0b1;
vehicle_state.errors.err_scs = (data[2] >> 2) & 0b1;
vehicle_state.errors.err_con_mon = (data[2] >> 3) & 0b1;
vehicle_state.errors.err_initial_checkup = (data[2] >> 4) & 0b1;
vehicle_state.errors.err_inv_2 = (data[2] >> 5) & 0b1;
vehicle_state.errors.err_inv_1 = (data[2] >> 6) & 0b1;
vehicle_state.errors.err_ams = (data[2] >> 7) & 0b1;
vehicle_state.errors.err_sdc = (data[3] >> 0) & 0b1;
vehicle_state.sdc_status = (data[3] >> 1) & 0b1111;
vehicle_state.inv_2_ready = (data[3] >> 5) & 0b1;
vehicle_state.inv_1_ready = (data[3] >> 6) & 0b1;
vehicle_state.energy_per_lap = data[4] | (data[5] << 8);
vehicle_state.initial_checkup_state = data[6];
break;
case CAN_ID_FTCU_PARAM_CONFIRMED:
vehicle_state.last_param_confirmed = data[0];
tx_event_flags_set(&gui_update_events, GUI_UPDATE_PARAM_CONFIRMED, TX_OR);
break;
case CAN_ID_FTCU_TELEMETRIE1:
// TODO
break;
case CAN_ID_FTCU_TELEMETRIE2:
// TODO
break;
case CAN_ID_PDU_RESPONSE:
// TODO
break;
case CAN_ID_PDU_CURRENT1:
// TODO
break;
case CAN_ID_PDU_CURRENT2:
// TODO
break;
case CAN_ID_PDU_CURRENT3:
// TODO
break;
case CAN_ID_PDU_CURRENT4:
// TODO
break;
case CAN_ID_DASHBOARD:
// TODO
break;
case CAN_ID_SSU:
// TODO
break;
case CAN_ID_SHUNT_CURRENT:
// The first two bytes of shunt result messages are metadata
ptr = &data[2];
vehicle_state.ts_current = ftcan_unmarshal_signed(&ptr, 4) * 0.001f;
break;
case CAN_ID_SHUNT_VOLTAGE1:
ptr = &data[2];
vehicle_state.ts_voltage_bat = ftcan_unmarshal_signed(&ptr, 4) * -0.001f;
break;
case CAN_ID_SHUNT_VOLTAGE2:
ptr = &data[2];
vehicle_state.ts_voltage_veh = vehicle_state.ts_voltage_bat - (ftcan_unmarshal_signed(&ptr, 4) * -0.001f);
break;
case CAN_ID_SHUNT_TEMPERATURE:
ptr = &data[2];
vehicle_state.shunt_temperature = ftcan_unmarshal_signed(&ptr, 4) * 0.1f;
break;
case CAN_ID_TTS_FL:
// TODO
break;
case CAN_ID_TTS_FR:
// TODO
break;
case CAN_ID_TTS_RL:
// TODO
break;
case CAN_ID_TTS_RR:
// TODO
break;
case CAN_ID_INVERTER_VELOCITY:
vehicle_state.inv_velocity_1 = (data[0] | (data[1] << 8) | (data[2] << 16) | (data[3] << 24)) * 0.001f;
vehicle_state.inv_velocity_2 = (data[4] | (data[5] << 8) | (data[6] << 16) | (data[7] << 24)) * 0.001f;
break;
case CAN_ID_INVERTER_TORQUE_WANTED:
vehicle_state.inv_torque_demanded_1 = data[0] | (data[1] << 8);
vehicle_state.inv_torque_demanded_2 = data[2] | (data[3] << 8);
vehicle_state.inv_torque_desired_1 = data[4] | (data[5] << 8);
vehicle_state.inv_torque_desired_2 = data[6] | (data[7] << 8);
break;
case CAN_ID_INVERTER_TEMPERATURE:
vehicle_state.temps.inv_1 = data[0] | (data[1] << 8);
vehicle_state.temps.mot_1 = data[2] | (data[3] << 8);
vehicle_state.temps.inv_2 = data[4] | (data[5] << 8);
vehicle_state.temps.mot_2 = data[6] | (data[7] << 8);
break;
case CAN_ID_INVERTER_TORQUE_ACTUAL:
vehicle_state.inv_control_word_1 = data[0] | (data[1] << 8);
vehicle_state.inv_torque_actual_1 = data[2] | (data[3] << 8);
vehicle_state.inv_control_word_2 = data[4] | (data[5] << 8);
vehicle_state.inv_torque_actual_2 = data[6] | (data[7] << 8);
break;
case CAN_ID_INVERTER_ERRORS_WARNINGS:
vehicle_state.inv_errors_1 = data[0] | (data[1] << 8);
vehicle_state.inv_warnings_1 = data[2] | (data[3] << 8);
vehicle_state.inv_errors_2 = data[4] | (data[5] << 8);
vehicle_state.inv_warnings_2 = data[6] | (data[7] << 8);
break;
}
tx_event_flags_set(&gui_update_events, GUI_UPDATE_VEHICLE_STATE, TX_OR);
}

56
Core/Src/vehicle_state.c
View File

@ -2,40 +2,28 @@
VehicleState vehicle_state;
const char *inichkstate_str(IniChkState state) {
switch (vehicle_state.ini_chk_state) {
case INICHK_START:
return "START";
case INICHK_WD_CHECK:
return "WD CHK";
case INICHK_WD_OK:
return "WD OK";
case INICHK_POS_CHK_1:
return "POS CHK 1";
case INICHK_ASB_CHECK_A_1:
return "ASB CHK A1";
case INICHK_ASB_CHECK_A_2:
return "ASB CHK A2";
case INICHK_SWITCH_B:
return "SWITCH B";
case INICHK_ASB_CHECK_B_1:
return "ASB CHK B1";
case INICHK_ASB_CHECK_B_2:
return "ASB CHK B2";
case INICHK_SWITCH_A:
return "SWITCH A";
case INICHK_ASB_CHECK_AB_1:
return "ASB CHK AB1";
case INICHK_ASB_CHECK_AB_2:
return "ASB CHK AB2";
case INICHK_POS_CHK_2:
return "POS CHK 2";
case INICHK_WAIT_TS:
return "WAIT TS";
case INICHK_DONE:
return "DONE";
case INICHK_ERROR:
return "ERROR";
const char *inichkstate_str(InitialCheckupState state) {
switch (vehicle_state.initial_checkup_state) {
case INICHK_START:
return "START";
case INICHK_WD_CHECK:
return "WD CHK";
case INICHK_WD_OK:
return "WD OK";
case INICHK_ASB_CHECK_1:
return "ABC CHK 1";
case INICHK_ASB_CHECK_2:
return "ASB CHK 2";
case INICHK_WAIT_TS:
return "WAIT TS";
case INICHK_EBS_CHECK_A:
return "EBS CHK A";
case INICHK_EBS_CHECK_B:
return "EBS CHK B";
case INICHK_DONE:
return "DONE";
case INICHK_ERROR:
return "ERROR";
}
return "UNKNOWN";
}