diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 290150657e..41ce5fb7dc 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -27,6 +27,7 @@ #define MSG_SUBARU_Wheel_Speeds 0x13a #define MSG_SUBARU_ES_LKAS 0x122 +#define MSG_SUBARU_ES_LKAS_ANGLE 0x124 #define MSG_SUBARU_ES_Brake 0x220 #define MSG_SUBARU_ES_Distance 0x221 #define MSG_SUBARU_ES_Status 0x222 @@ -70,6 +71,7 @@ static bool subaru_gen2 = false; static bool subaru_longitudinal = false; +static bool subaru_lkas_angle = false; static uint32_t subaru_get_checksum(const CANPacket_t *to_push) { return (uint8_t)GET_BYTE(to_push, 0); @@ -91,7 +93,8 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *to_push) { static void subaru_rx_hook(const CANPacket_t *to_push) { const int bus = GET_BUS(to_push); - const int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; + const int alt_main_bus = (subaru_gen2 || subaru_lkas_angle) ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; + const int stock_lkas_msg = subaru_lkas_angle ? MSG_SUBARU_ES_LKAS_ANGLE : MSG_SUBARU_ES_LKAS; int addr = GET_ADDR(to_push); if ((addr == MSG_SUBARU_Steering_Torque) && (bus == SUBARU_MAIN_BUS)) { @@ -107,9 +110,18 @@ static void subaru_rx_hook(const CANPacket_t *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) { - bool cruise_engaged = GET_BIT(to_push, 41U); - pcm_cruise_check(cruise_engaged); + if (subaru_lkas_angle) { + // LKAS Angle cars use different message + if ((addr == MSG_SUBARU_ES_DashStatus) && (bus == SUBARU_CAM_BUS)) { + bool cruise_engaged = GET_BIT(to_push, 36U); + pcm_cruise_check(cruise_engaged); + } + } else { + if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) { + bool cruise_engaged = GET_BIT(to_push, 41U); + pcm_cruise_check(cruise_engaged); + + } } // update vehicle moving with any non-zero wheel speed @@ -133,12 +145,25 @@ static void subaru_rx_hook(const CANPacket_t *to_push) { } generic_rx_checks((addr == MSG_SUBARU_ES_LKAS) && (bus == SUBARU_MAIN_BUS)); + generic_rx_checks((addr == stock_lkas_msg) && (bus == SUBARU_MAIN_BUS)); } static bool subaru_tx_hook(const CANPacket_t *to_send) { const SteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70); const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40); + const SteeringLimits SUBARU_ANGLE_STEERING_LIMITS = { + .angle_deg_to_can = 100., + .angle_rate_up_lookup = { + {0., 15., 15.}, + {5., .8, .8} + }, + .angle_rate_down_lookup = { + {0., 15., 15.}, + {5., .4, .4} + }, + }; + const LongitudinalLimits SUBARU_LONG_LIMITS = { .min_gas = 808, // appears to be engine braking .max_gas = 3400, // approx 2 m/s^2 when maxing cruise_rpm and cruise_throttle @@ -164,6 +189,14 @@ static bool subaru_tx_hook(const CANPacket_t *to_send) { violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits); } + if (addr == MSG_SUBARU_ES_LKAS_ANGLE) { + int desired_angle = GET_BYTES(to_send, 5, 3) & 0x1FFFFU; + desired_angle = -1 * to_signed(desired_angle, 17); + bool lkas_request = GET_BIT(to_send, 12U); + + violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS); + } + // check es_brake brake_pressure limits if (addr == MSG_SUBARU_ES_Brake) { int es_brake_pressure = GET_BYTES(to_send, 2, 2); @@ -210,13 +243,15 @@ static bool subaru_tx_hook(const CANPacket_t *to_send) { static int subaru_fwd_hook(int bus_num, int addr) { int bus_fwd = -1; + const int stock_lkas_msg = subaru_lkas_angle ? MSG_SUBARU_ES_LKAS_ANGLE : MSG_SUBARU_ES_LKAS; + if (bus_num == SUBARU_MAIN_BUS) { bus_fwd = SUBARU_CAM_BUS; // to the eyesight camera } if (bus_num == SUBARU_CAM_BUS) { // Global platform - bool block_lkas = ((addr == MSG_SUBARU_ES_LKAS) || + bool block_lkas = ((addr == stock_lkas_msg) || (addr == MSG_SUBARU_ES_DashStatus) || (addr == MSG_SUBARU_ES_LKAS_State) || (addr == MSG_SUBARU_ES_Infotainment)); @@ -254,6 +289,11 @@ static safety_config subaru_init(uint16_t param) { SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS() }; + static const CanMsg SUBARU_LKAS_ANGLE_TX_MSGS[] = { + SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE) + SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS) + }; + static RxCheck subaru_rx_checks[] = { SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS) }; @@ -263,8 +303,10 @@ static safety_config subaru_init(uint16_t param) { }; const uint16_t SUBARU_PARAM_GEN2 = 1; + const uint16_t SUBARU_PARAM_LKAS_ANGLE = 4; subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2); + subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE); #ifdef ALLOW_DEBUG const uint16_t SUBARU_PARAM_LONGITUDINAL = 2; @@ -272,7 +314,10 @@ static safety_config subaru_init(uint16_t param) { #endif safety_config ret; - if (subaru_gen2) { + if (subaru_lkas_angle) { + ret = BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS); + } + else if (subaru_gen2) { ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \ BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_TX_MSGS); } else { diff --git a/python/__init__.py b/python/__init__.py index 88ccf897a1..83c870d26c 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -214,6 +214,7 @@ class Panda: FLAG_SUBARU_GEN2 = 1 FLAG_SUBARU_LONG = 2 + FLAG_SUBARU_LKAS_ANGLE = 4 FLAG_SUBARU_PREGLOBAL_REVERSED_DRIVER_TORQUE = 1 diff --git a/tests/safety/common.py b/tests/safety/common.py index 884f21a360..67325d88a9 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -759,6 +759,10 @@ def test_tx_hook_on_wrong_safety_mode(self): continue if attr.startswith('TestSubaruGen') and current_test.startswith('TestSubaruGen'): continue + if attr.startswith('TestSubaruAngle') and current_test.startswith('TestSubaruGen'): + continue + if attr.startswith('TestSubaruGen') and current_test.startswith('TestSubaruAngle'): + continue if attr.startswith('TestSubaruPreglobal') and current_test.startswith('TestSubaruPreglobal'): continue if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}): diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index 7d07f79aaf..77167946b9 100755 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -68,7 +68,7 @@ class TestSubaruSafetyBase(common.PandaCarSafetyTest): ALT_MAIN_BUS = SUBARU_MAIN_BUS ALT_CAM_BUS = SUBARU_CAM_BUS - DEG_TO_CAN = 100 + DEG_TO_CAN = 100. INACTIVE_GAS = 1818 @@ -167,6 +167,33 @@ def _torque_cmd_msg(self, torque, steer_req=1): return self.packer.make_can_msg_panda("ES_LKAS", SUBARU_MAIN_BUS, values) +class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafetyTest): + ALT_MAIN_BUS = SUBARU_ALT_BUS + + TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS, SubaruMsg.ES_LKAS_ANGLE) + RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_LKAS,)} + FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr(SubaruMsg.ES_LKAS_ANGLE) + + FLAGS = Panda.FLAG_SUBARU_LKAS_ANGLE + + ANGLE_RATE_BP = [0, 15] + ANGLE_RATE_UP = [5, 0.15] + ANGLE_RATE_DOWN = [5, 0.4] + + def _angle_cmd_msg(self, angle, enabled=1): + values = {"LKAS_Output": angle, "LKAS_Request": enabled} + return self.packer.make_can_msg_panda("ES_LKAS_ANGLE", 0, values) + + def _angle_meas_msg(self, angle): + values = {"Steering_Angle": angle} + return self.packer.make_can_msg_panda("Steering_Torque", 0, values) + + # need to use ES_DashStatus Message + def _pcm_status_msg(self, enable): + values = {"Cruise_Activated": enable} + return self.packer.make_can_msg_panda("ES_DashStatus", self.ALT_CAM_BUS, values) + + class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase): FLAGS = 0 TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) @@ -186,6 +213,13 @@ class TestSubaruGen2TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSaf TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS) +class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase): + ALT_MAIN_BUS = SUBARU_ALT_BUS + + FLAGS = Panda.FLAG_SUBARU_GEN2 | Panda.FLAG_SUBARU_LKAS_ANGLE + TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS) + lkas_tx_msgs(SUBARU_MAIN_BUS, SubaruMsg.ES_LKAS_ANGLE) + + class TestSubaruGen1LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruTorqueSafetyBase): FLAGS = Panda.FLAG_SUBARU_LONG TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) + long_tx_msgs(SUBARU_MAIN_BUS)