update for new CAN dbc (incomplete), add more NamedFields, allow mission select without confirmation, set default mission select state to manual
This commit is contained in:
@ -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);
|
||||
}
|
||||
|
||||
@ -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";
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user