diff --git a/Core/Src/params.c b/Core/Src/params.c index 9397194..1816d97 100644 --- a/Core/Src/params.c +++ b/Core/Src/params.c @@ -5,8 +5,10 @@ /** * Decrements the given value if it is above the minimum allowed value */ -#define DEC_IF_ABOVE(param_val, min_val, decr_amt) ((param_val) = ((param_val) - (decr_amt) ) > (min_val) ? ((param_val) - (decr_amt)) : (min_val)) -#define INC_IF_BELOW(param_val, max_val, incr_amt) ((param_val) = ((param_val) + (incr_amt)) < (max_val) ? ((param_val) + (incr_amt)) : (max_val)) +// TODO these functions take into account that the parameters are unsigned, it's definitely better to have them +// signed but would need to be tested with the autobox +#define DEC_IF_ABOVE(param_val, min_val, decr_amt) ((param_val) = (((int)(param_val) - (int)(decr_amt)) > (int)(min_val)) ? ((param_val) - (decr_amt)) : (min_val)) +#define INC_IF_BELOW(param_val, max_val, incr_amt) ((param_val) = (((param_val) + (incr_amt)) < (max_val)) ? ((param_val) + (incr_amt)) : (max_val)) Params params = {0}; diff --git a/Core/Src/ui.c b/Core/Src/ui.c index 7de42db..4118a3e 100644 --- a/Core/Src/ui.c +++ b/Core/Src/ui.c @@ -13,7 +13,7 @@ #include "leds.h" #define DRS_BUTTON_IDX (6) -#define DRS_PRESS_WAIT_CYCLES (10) +#define DRS_PRESS_WAIT_CYCLES (1000) static int drs_press_buf_cycles = 0; void ui_thread_entry(ULONG _) { diff --git a/Core/Src/vehicle.c b/Core/Src/vehicle.c index 6ce8b12..a595d05 100644 --- a/Core/Src/vehicle.c +++ b/Core/Src/vehicle.c @@ -1,16 +1,14 @@ #include "vehicle.h" -#include "main.h" -#include "ui.h" -#include "vehicle_state.h" - #include "can-halal.h" - +#include "main.h" #include "stm32h7xx.h" #include "stm32h7xx_hal.h" #include "stm32h7xx_hal_fdcan.h" #include "stm32h7xx_hal_gpio.h" #include "tx_api.h" +#include "ui.h" +#include "vehicle_state.h" #define CAN_ID_AMS_SLAVE_PANIC 0x9 #define CAN_ID_AMS_STATUS 0xA @@ -98,7 +96,8 @@ void vehicle_broadcast_param(ParamType param, int32_t value) { 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); + (button_states[2] << 1) | (button_states[3] << 3) | + (button_states[6] << 4); ftcan_transmit(CAN_ID_STW_BUTTONS, &data, 1); } @@ -112,142 +111,144 @@ void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) { } } 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; - } + 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; + } } } tx_event_flags_set(&gui_update_events, GUI_UPDATE_VEHICLE_STATE, TX_OR);