diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 581dc50b7f..e937c76b3d 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -34,6 +34,7 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = { #define MSG_SUBARU_Brake_Status 0x13c #define MSG_SUBARU_CruiseControl 0x240 #define MSG_SUBARU_Throttle 0x40 +#define MSG_SUBARU_Steering_Torque_2 0x118 #define MSG_SUBARU_Steering_Torque 0x119 #define MSG_SUBARU_Wheel_Speeds 0x13a @@ -75,6 +76,7 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = { #define SUBARU_COMMON_RX_CHECKS(alt_bus) \ {.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Steering_Torque_2, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \ {.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \ {.msg = {{MSG_SUBARU_Wheel_Speeds, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \ {.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \ @@ -138,12 +140,14 @@ static void subaru_rx_hook(const CANPacket_t *to_push) { const int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; int addr = GET_ADDR(to_push); - if ((addr == MSG_SUBARU_Steering_Torque) && (bus == SUBARU_MAIN_BUS)) { + if ((addr == MSG_SUBARU_Steering_Torque_2) && (bus == SUBARU_MAIN_BUS)) { int torque_driver_new; - torque_driver_new = ((GET_BYTES(to_push, 0, 4) >> 16) & 0x7FFU); + torque_driver_new = ((GET_BYTES(to_push, 0, 4) >> 45) & 0x7FFU); torque_driver_new = -1 * to_signed(torque_driver_new, 11); update_sample(&torque_driver, torque_driver_new); + } + if ((addr == MSG_SUBARU_Steering_Torque) && (bus == SUBARU_MAIN_BUS)) { int angle_meas_new = (GET_BYTES(to_push, 4, 2) & 0xFFFFU); // convert Steering_Torque -> Steering_Angle to centidegrees, to match the ES_LKAS_ANGLE angle request units angle_meas_new = ROUND(to_signed(angle_meas_new, 16) * -2.17);