102 lines
3.5 KiB
C
102 lines
3.5 KiB
C
#include "vehicle.h"
|
|
|
|
#include "main.h"
|
|
#include "ui.h"
|
|
|
|
#include "FT_CAN_AL.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_STATUS 0xA
|
|
#define CAN_ID_MISSION_SELECTED 0x400
|
|
#define CAN_ID_AS_MISSION_FB 0x410
|
|
#define CAN_ID_STW_STATUS 0x412
|
|
#define CAN_ID_SHUNT_CURRENT 0x521
|
|
#define CAN_ID_SHUNT_VOLTAGE2 0x523
|
|
#define CAN_ID_SHUNT_VOLTAGE3 0x524
|
|
#define CAN_AMS_STATUS_VOLTAGE_FACTOR (1e-3 * 0x100)
|
|
#define CAN_AMS_STATUS_TEMP_FACTOR (0.0625 * 0x10)
|
|
|
|
VehicleState vehicle_state = {0};
|
|
|
|
void vehicle_thread_entry(ULONG hfdcan_addr) {
|
|
ftcan_init((void *)hfdcan_addr);
|
|
ftcan_add_filter(CAN_ID_AMS_STATUS, 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_VOLTAGE2, 0x7FF);
|
|
ftcan_add_filter(CAN_ID_SHUNT_VOLTAGE3, 0x7FF);
|
|
|
|
while (1) {
|
|
uint8_t data[] = {0xFF, 0xEE};
|
|
ftcan_transmit(0x456, data, 2);
|
|
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 ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
|
|
VehicleUpdate msg;
|
|
switch (id) {
|
|
case CAN_ID_AMS_STATUS:
|
|
vehicle_state.ts_state = data[0] & 0x7F;
|
|
vehicle_state.soc = data[1];
|
|
vehicle_state.min_cell_volt = data[2] * CAN_AMS_STATUS_VOLTAGE_FACTOR;
|
|
vehicle_state.max_cell_temp = data[3] * CAN_AMS_STATUS_TEMP_FACTOR;
|
|
msg = VEH_UPD_AMS;
|
|
break;
|
|
case CAN_ID_AS_MISSION_FB:
|
|
vehicle_state.active_mission = data[0] & 0b111;
|
|
msg = VEH_UPD_MISSION;
|
|
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;
|
|
msg = VEH_UPD_STATUS;
|
|
break;
|
|
case CAN_ID_SHUNT_CURRENT:
|
|
vehicle_state.ts_current = ftcan_unmarshal_signed(&data, 6) * 1e-3;
|
|
msg = VEH_UPD_TS_CURRENT;
|
|
break;
|
|
case CAN_ID_SHUNT_VOLTAGE2:
|
|
vehicle_state.ts_voltage_veh = ftcan_unmarshal_signed(&data, 6) * 1e-3;
|
|
msg = VEH_UPD_TS_VOLTAGE_VEH;
|
|
break;
|
|
case CAN_ID_SHUNT_VOLTAGE3:
|
|
vehicle_state.ts_voltage_bat = ftcan_unmarshal_signed(&data, 6) * 1e-3;
|
|
msg = VEH_UPD_TS_VOLTAGE_BAT;
|
|
break;
|
|
}
|
|
tx_queue_send(&vehicle_update_queue, &msg, TX_NO_WAIT);
|
|
}
|