From b5774cfddf4a85298a122b60a8fed462d196fde3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Fri, 21 Feb 2025 23:30:48 -0800 Subject: [PATCH] Add panda safety code --- opendbc/car/subaru/values.py | 1 + opendbc/safety/safety/safety_subaru.h | 57 +++++++++++++++++++++++---- opendbc/safety/tests/test_subaru.py | 27 +++++++++++++ 3 files changed, 77 insertions(+), 8 deletions(-) diff --git a/opendbc/car/subaru/values.py b/opendbc/car/subaru/values.py index 68171c5dd5b..71c7cfbfbde 100644 --- a/opendbc/car/subaru/values.py +++ b/opendbc/car/subaru/values.py @@ -57,6 +57,7 @@ class SubaruSafetyFlags(IntFlag): GEN2 = 1 LONG = 2 PREGLOBAL_REVERSED_DRIVER_TORQUE = 4 + ANGLE = 8 class SubaruFlags(IntFlag): diff --git a/opendbc/safety/safety/safety_subaru.h b/opendbc/safety/safety/safety_subaru.h index 290150657e5..9815b734cb7 100644 --- a/opendbc/safety/safety/safety_subaru.h +++ b/opendbc/safety/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,16 @@ 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) { + 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 @@ -132,13 +142,25 @@ static void subaru_rx_hook(const CANPacket_t *to_push) { gas_pressed = GET_BYTE(to_push, 4) != 0U; } - 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 +186,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 +240,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 +286,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,16 +300,20 @@ static safety_config subaru_init(uint16_t param) { }; const uint16_t SUBARU_PARAM_GEN2 = 1; + const uint16_t SUBARU_PARAM_LONGITUDINAL = 2; + const uint16_t SUBARU_PARAM_LKAS_ANGLE = 8; 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; subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL); #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/opendbc/safety/tests/test_subaru.py b/opendbc/safety/tests/test_subaru.py index 33cb6c3934e..a89df58280b 100755 --- a/opendbc/safety/tests/test_subaru.py +++ b/opendbc/safety/tests/test_subaru.py @@ -169,6 +169,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 = SubaruSafetyFlags.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)