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

Display steering angle, speed & cones in AMI

Browse Source
This commit is contained in:
Jasper Blanckenburg
2023-08-01 19:21:33 +02:00
parent 6d70d12148
commit 2b54a8ffe8
42 changed files with 1351 additions and 371 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
Core/Inc/vehicle_state.h
View File

@ -8,6 +8,8 @@ extern "C" {
#include "stw_defines.h"
#include <stdint.h>
#define NUM_CONES 12
typedef enum {
TS_INACTIVE = 0,
TS_ACTIVE = 1,
@ -88,6 +90,11 @@ typedef struct {
float bat_r;
} Temperatures;
typedef struct {
uint16_t x;
uint16_t y;
} ConePosition;
typedef struct {
TSState ts_state;
ASState as_state;
@ -151,6 +158,14 @@ typedef struct {
float brake_press_r;
float distance_total;
uint32_t last_jetson_msg;
uint32_t last_epsc_msg;
float desired_angle;
float measured_angle;
float desired_speed;
ConePosition cone_pos[NUM_CONES];
} VehicleState;
extern VehicleState vehicle_state;

236
Core/Src/vehicle.c
View File

@ -15,39 +15,51 @@
#define CAN_ID_AMS_SLAVE_PANIC 0x9
#define CAN_ID_AMS_STATUS 0xA
#define CAN_ID_AMS_ERROR 0xC
#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_EPSC_OUT 0x321
#define CAN_ID_MISSION_SELECTED 0x400
#define CAN_ID_STW_BUTTONS 0x401
#define CAN_ID_STW_PARAM_SET 0x402
#define CAN_ID_AS_MISSION_FB 0x410
#define CAN_ID_STW_STATUS 0x412
#define CAN_ID_STW_CONES_BASE 0x414
#define CAN_ID_STW_CONES_MASK 0x7FC
#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_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_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
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_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_EPSC_OUT, 0x7FF);
ftcan_add_filter(CAN_ID_AS_MISSION_FB, 0x7FF);
ftcan_add_filter(CAN_ID_STW_STATUS, 0x7FF);
ftcan_add_filter(CAN_ID_STW_CONES_BASE, CAN_ID_STW_CONES_MASK);
ftcan_add_filter(CAN_ID_SHUNT_CURRENT, 0x7FF);
ftcan_add_filter(CAN_ID_SHUNT_VOLTAGE1, 0x7FF);
ftcan_add_filter(CAN_ID_SHUNT_VOLTAGE2, 0x7FF);
@ -78,107 +90,129 @@ void vehicle_broadcast_buttons(GPIO_PinState *button_states) {
void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
const uint8_t *ptr;
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;
// 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_AMS_ERROR:
vehicle_state.last_ams_error.kind = data[0];
vehicle_state.last_ams_error.arg = data[1];
break;
case CAN_ID_ABX_DRIVER:
vehicle_state.brake_press_f = (data[1] | ((data[2] & 0x0F) << 8)) * 0.1;
vehicle_state.brake_press_r = ((data[2] >> 4) | (data[3] << 4)) * 0.1;
vehicle_state.speed = data[5] * CAN_ABX_DRIVER_SPEED_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_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_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;
}
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;
// 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_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.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)) * 0.1;
vehicle_state.brake_press_r = ((data[2] >> 4) | (data[3] << 4)) * 0.1;
vehicle_state.speed = data[5] * CAN_ABX_DRIVER_SPEED_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_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;
}
}
}
tx_event_flags_set(&gui_update_events, GUI_UPDATE_VEHICLE_STATE, TX_OR);
}