From 2e1ec5f467675443d27cabe505b560b902ab1dd6 Mon Sep 17 00:00:00 2001 From: "Jasper v. Blanckenburg" Date: Tue, 25 Apr 2023 14:03:53 +0200 Subject: [PATCH] Read shunt messages correctly --- Core/Src/vehicle.c | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/Core/Src/vehicle.c b/Core/Src/vehicle.c index a81a58d..3247853 100644 --- a/Core/Src/vehicle.c +++ b/Core/Src/vehicle.c @@ -18,8 +18,8 @@ #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_ID_SHUNT_VOLTAGE3 0x524 #define CAN_AMS_STATUS_VOLTAGE_FACTOR 1e-4 #define CAN_AMS_STATUS_TEMP_FACTOR 0.0625 @@ -31,8 +31,8 @@ void vehicle_thread_entry(ULONG hfdcan_addr) { 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); - ftcan_add_filter(CAN_ID_SHUNT_VOLTAGE3, 0x7FF); while (1) { tx_thread_sleep(10); @@ -97,15 +97,24 @@ void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) { vehicle_state.errors.err_invl = (data[5] >> 3) & 1; vehicle_state.errors.err_invr = (data[5] >> 4) & 1; break; - case CAN_ID_SHUNT_CURRENT: - vehicle_state.ts_current = ftcan_unmarshal_signed(&data, 6) * 1e-3; + 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_VOLTAGE2: - vehicle_state.ts_voltage_veh = ftcan_unmarshal_signed(&data, 6) * 1e-3; + } + 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_VOLTAGE3: - vehicle_state.ts_voltage_bat = ftcan_unmarshal_signed(&data, 6) * 1e-3; + } + 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); }