177 lines
6.6 KiB
C
177 lines
6.6 KiB
C
#include "vehicle.h"
|
|
|
|
#include "main.h"
|
|
#include "ui.h"
|
|
#include "vehicle_state.h"
|
|
|
|
#include "can-halal.h"
|
|
|
|
#include "stm32h7xx.h"
|
|
#include "stm32h7xx_hal.h"
|
|
#include "stm32h7xx_hal_fdcan.h"
|
|
#include "stm32h7xx_hal_gpio.h"
|
|
#include "tx_api.h"
|
|
|
|
#define CAN_ID_AMS_SLAVE_PANIC 0x9
|
|
#define CAN_ID_AMS_STATUS 0xA
|
|
#define CAN_ID_AMS_ERROR 0xC
|
|
#define CAN_ID_ABX_DRIVER 0x101
|
|
#define CAN_ID_ABX_BRAKE_T 0x105
|
|
#define CAN_ID_CS_INTERNAL 0x108
|
|
#define CAN_ID_ABX_MISC 0x109
|
|
#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_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_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
|
|
|
|
void vehicle_thread_entry(ULONG hfdcan_addr) {
|
|
memset(&vehicle_state, 0, sizeof(vehicle_state));
|
|
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_ABX_DRIVER, 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_AS_MISSION_FB, 0x7FF);
|
|
ftcan_add_filter(CAN_ID_STW_STATUS, 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);
|
|
|
|
while (1) {
|
|
tx_thread_sleep(10);
|
|
}
|
|
}
|
|
|
|
void vehicle_select_mission(Mission mission) {
|
|
uint8_t mission_int = mission;
|
|
ftcan_transmit(CAN_ID_MISSION_SELECTED, &mission_int, 1);
|
|
}
|
|
|
|
void vehicle_broadcast_param(ParamType param, int32_t value) {
|
|
uint8_t data[5];
|
|
uint8_t *ptr = data;
|
|
ptr = ftcan_marshal_unsigned(ptr, param, 1);
|
|
ptr = ftcan_marshal_signed(ptr, value, 4);
|
|
ftcan_transmit(CAN_ID_STW_PARAM_SET, data, 5);
|
|
}
|
|
|
|
void vehicle_broadcast_buttons(GPIO_PinState *button_states) {
|
|
uint8_t data = (button_states[0] << 2) | (button_states[1] << 0) |
|
|
(button_states[2] << 1) | (button_states[3] << 3);
|
|
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;
|
|
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 = 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) << 4)) * 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_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;
|
|
// TODO: uncomment when we have a right motor
|
|
// 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;
|
|
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);
|
|
}
|