diff --git a/docs/CARS.md b/docs/CARS.md index 88514042286..848f20bde98 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -266,6 +266,7 @@ |Subaru|Ascent 2023|All|[Dashcam mode](#dashcam)| |Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|[Upstream](#upstream)| |Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|[Upstream](#upstream)| +|Subaru|Crosstrek 2025|All|[Dashcam mode](#dashcam)| |Subaru|Crosstrek Hybrid 2020|EyeSight Driver Assistance|[Dashcam mode](#dashcam)| |Subaru|Forester 2017-18|EyeSight Driver Assistance|[Dashcam mode](#dashcam)| |Subaru|Forester 2019-21|All|[Upstream](#upstream)| diff --git a/opendbc/car/subaru/carcontroller.py b/opendbc/car/subaru/carcontroller.py index 3b9abf5917b..52859c4c6db 100644 --- a/opendbc/car/subaru/carcontroller.py +++ b/opendbc/car/subaru/carcontroller.py @@ -1,7 +1,7 @@ import numpy as np from opendbc.can import CANPacker from opendbc.car import Bus, make_tester_present_msg -from opendbc.car.lateral import apply_driver_steer_torque_limits, common_fault_avoidance +from opendbc.car.lateral import apply_driver_steer_torque_limits, common_fault_avoidance, apply_std_steer_angle_limits from opendbc.car.interfaces import CarControllerBase from opendbc.car.subaru import subarucan from opendbc.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags @@ -16,6 +16,7 @@ class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) self.apply_torque_last = 0 + self.apply_angle_last = 0 self.cruise_button_prev = 0 self.steer_rate_counter = 0 @@ -23,6 +24,48 @@ def __init__(self, dbc_names, CP): self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + def handle_angle_lateral(self, CC, CS): + apply_steer = apply_std_steer_angle_limits( + CC.actuators.steeringAngleDeg, + self.apply_angle_last, + CS.out.vEgoRaw, + CS.out.steeringAngleDeg, + CC.latActive, + self.p.ANGLE_LIMITS + ) + + if not CC.latActive: + apply_steer = CS.out.steeringAngleDeg + + self.apply_angle_last = apply_steer + return subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive) + + def handle_torque_lateral(self, CC, CS): + apply_torque = int(round(CC.actuators.torque * self.p.STEER_MAX)) + + new_torque = int(round(apply_torque)) + apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.p) + + if not CC.latActive: + apply_torque = 0 + + msg = None + if self.CP.flags & SubaruFlags.PREGLOBAL: + msg = subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_torque, CC.latActive) + else: + apply_steer_req = CC.latActive + + if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED: + # Steering rate fault prevention + self.steer_rate_counter, apply_steer_req = \ + common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req, + self.steer_rate_counter, MAX_STEER_RATE_FRAMES) + + msg = subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req) + + self.apply_torque_last = apply_torque + return msg + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -32,30 +75,10 @@ def update(self, CC, CS, now_nanos): # *** steering *** if (self.frame % self.p.STEER_STEP) == 0: - apply_torque = int(round(actuators.torque * self.p.STEER_MAX)) - - # limits due to driver torque - - new_torque = int(round(apply_torque)) - apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.p) - - if not CC.latActive: - apply_torque = 0 - - if self.CP.flags & SubaruFlags.PREGLOBAL: - can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_torque, CC.latActive)) + if self.CP.flags & SubaruFlags.LKAS_ANGLE: + can_sends.append(self.handle_angle_lateral(CC, CS)) else: - apply_steer_req = CC.latActive - - if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED: - # Steering rate fault prevention - self.steer_rate_counter, apply_steer_req = \ - common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req, - self.steer_rate_counter, MAX_STEER_RATE_FRAMES) - - can_sends.append(subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req)) - - self.apply_torque_last = apply_torque + can_sends.append(self.handle_torque_lateral(CC, CS)) # *** longitudinal *** @@ -137,6 +160,7 @@ def update(self, CC, CS, now_nanos): can_sends.append(subarucan.create_es_static_2(self.packer)) new_actuators = actuators.as_builder() + new_actuators.steeringAngleDeg = self.apply_angle_last new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX new_actuators.torqueOutputCan = self.apply_torque_last diff --git a/opendbc/car/subaru/interface.py b/opendbc/car/subaru/interface.py index 5b16f17d253..96bfd36b767 100644 --- a/opendbc/car/subaru/interface.py +++ b/opendbc/car/subaru/interface.py @@ -18,7 +18,15 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, alp # - replacement for ES_Distance so we can cancel the cruise control # - to find the Cruise_Activated bit from the car # - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc) - ret.dashcamOnly = bool(ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID)) + # for LKAS_ANGLE CARS to be upstreamed, we need: + # - validate angle safety + ret.dashcamOnly = bool((ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.HYBRID)) or + ((ret.flags & SubaruFlags.LKAS_ANGLE) and is_release)) + + if candidate == CAR.SUBARU_FORESTER_2022: + # Gen 1 LKAS angle not tested, can undashcam if not release once we see a test route + ret.dashcamOnly = True + ret.autoResumeSng = False # Detect infotainment message sent from the camera @@ -42,6 +50,7 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, alp if ret.flags & SubaruFlags.LKAS_ANGLE: ret.steerControlType = structs.CarParams.SteerControlType.angle + ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LKAS_ANGLE.value elif candidate == CAR.SUBARU_ASCENT: ret.steerActuatorDelay = 0.3 # end-to-end angle controller diff --git a/opendbc/car/subaru/values.py b/opendbc/car/subaru/values.py index a702d39430f..7ba447dc1e4 100644 --- a/opendbc/car/subaru/values.py +++ b/opendbc/car/subaru/values.py @@ -2,6 +2,7 @@ from enum import Enum, IntFlag from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds +from opendbc.car.lateral import AngleSteeringLimits from opendbc.car.structs import CarParams from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 @@ -18,6 +19,12 @@ def __init__(self, CP): self.STEER_DRIVER_MULTIPLIER = 50 # weight driver torque heavily self.STEER_DRIVER_FACTOR = 1 # from dbc + self.ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits( + 545, + ([0., 5., 35.], [5., .8, .15,]), + ([0., 5., 35.], [5., .8, .15,]), + ) + if CP.flags & SubaruFlags.GLOBAL_GEN2: # TODO: lower rate limits, this reaches min/max in 0.5s which negatively affects tuning self.STEER_MAX = 1000 @@ -57,6 +64,7 @@ class SubaruSafetyFlags(IntFlag): GEN2 = 1 LONG = 2 PREGLOBAL_REVERSED_DRIVER_TORQUE = 4 + LKAS_ANGLE = 8 class SubaruFlags(IntFlag): diff --git a/opendbc/safety/modes/subaru.h b/opendbc/safety/modes/subaru.h index f5487a6773f..488acb0148c 100644 --- a/opendbc/safety/modes/subaru.h +++ b/opendbc/safety/modes/subaru.h @@ -22,10 +22,12 @@ #define MSG_SUBARU_Brake_Status 0x13cU #define MSG_SUBARU_CruiseControl 0x240U #define MSG_SUBARU_Throttle 0x40U +#define MSG_SUBARU_Steering_2 0x11aU #define MSG_SUBARU_Steering_Torque 0x119U #define MSG_SUBARU_Wheel_Speeds 0x13aU #define MSG_SUBARU_ES_LKAS 0x122U +#define MSG_SUBARU_ES_LKAS_ANGLE 0x124U #define MSG_SUBARU_ES_Brake 0x220U #define MSG_SUBARU_ES_Distance 0x221U #define MSG_SUBARU_ES_Status 0x222U @@ -70,8 +72,18 @@ {.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ {.msg = {{MSG_SUBARU_CruiseControl, alt_bus, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ +#define SUBARU_LKAS_ANGLE_RX_CHECKS(alt_bus) \ + {.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Wheel_Speeds, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_ES_Brake, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_ES_DashStatus, SUBARU_CAM_BUS, 8, 10U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Steering_2, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true }, { 0 }, { 0 }}}, \ + 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 *msg) { return (uint8_t)msg->data[0]; @@ -93,6 +105,16 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *msg) { static void subaru_rx_hook(const CANPacket_t *msg) { const unsigned int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; + if (subaru_lkas_angle) { + if ((msg->addr == MSG_SUBARU_Steering_2) && (msg->bus == SUBARU_MAIN_BUS)) { + uint32_t raw = GET_BYTES(msg, 3, 3); + raw &= 0x1FFFFU; + int angle_meas_new = -to_signed(raw, 17); + update_sample(&angle_meas, angle_meas_new); + } + } + + // non-zero, even for Angle-LKAS cars if ((msg->addr == MSG_SUBARU_Steering_Torque) && (msg->bus == SUBARU_MAIN_BUS)) { int torque_driver_new; torque_driver_new = ((GET_BYTES(msg, 0, 4) >> 16) & 0x7FFU); @@ -100,10 +122,17 @@ static void subaru_rx_hook(const CANPacket_t *msg) { update_sample(&torque_driver, torque_driver_new); } - // enter controls on rising edge of ACC, exit controls on ACC off - if ((msg->addr == MSG_SUBARU_CruiseControl) && (msg->bus == alt_main_bus)) { - bool cruise_engaged = (msg->data[5] >> 1) & 1U; - pcm_cruise_check(cruise_engaged); + if (subaru_lkas_angle) { + if ((msg->addr == MSG_SUBARU_ES_Brake) && (msg->bus == SUBARU_ALT_BUS)) { + bool cruise_engaged = (msg->data[4] >> 7) & 1U; + pcm_cruise_check(cruise_engaged); + } + } else { + // enter controls on rising edge of ACC, exit controls on ACC off + if ((msg->addr == MSG_SUBARU_CruiseControl) && (msg->bus == alt_main_bus)) { + bool cruise_engaged = (msg->data[5] >> 1) & 1U; + pcm_cruise_check(cruise_engaged); + } } // update vehicle moving with any non-zero wheel speed @@ -130,6 +159,18 @@ static void subaru_rx_hook(const CANPacket_t *msg) { static bool subaru_tx_hook(const CANPacket_t *msg) { const TorqueSteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70); const TorqueSteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40); + const AngleSteeringLimits SUBARU_ANGLE_STEERING_LIMITS = { + .max_angle = 545*100, + .angle_deg_to_can = 100., + .angle_rate_up_lookup = { + {0.0, 5.0, 35.0}, + {5.0, 0.8, 0.15} + }, + .angle_rate_down_lookup = { + {0.0, 5.0, 35.0}, + {5.0, 0.8, 0.15} + }, + }; const LongitudinalLimits SUBARU_LONG_LIMITS = { .min_gas = 808, // appears to be engine braking @@ -155,6 +196,16 @@ static bool subaru_tx_hook(const CANPacket_t *msg) { violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits); } + if (subaru_lkas_angle) { + if (msg->addr == MSG_SUBARU_ES_LKAS_ANGLE) { + int desired_angle = GET_BYTES(msg, 5, 3) & 0x1FFFFU; + desired_angle = -1 * to_signed(desired_angle, 17); + bool lkas_request = GET_BIT(msg, 12U); + + violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS); + } + } + // check es_brake brake_pressure limits if (msg->addr == MSG_SUBARU_ES_Brake) { int es_brake_pressure = GET_BYTES(msg, 2, 2); @@ -220,6 +271,18 @@ static safety_config subaru_init(uint16_t param) { SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS() }; + static const CanMsg SUBARU_LKAS_ANGLE_TX_MSGS[] = { + SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE) + SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS) + }; + + static const CanMsg SUBARU_LKAS_ANGLE_LONG_TX_MSGS[] = { + SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE) // lat + SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS) + SUBARU_COMMON_LONG_TX_MSGS(SUBARU_ALT_BUS) // long + SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS() + }; + static RxCheck subaru_rx_checks[] = { SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS) }; @@ -228,17 +291,26 @@ static safety_config subaru_init(uint16_t param) { SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS) }; - const uint16_t SUBARU_PARAM_GEN2 = 1; + static RxCheck subaru_lkas_angle_rx_checks[] = { + SUBARU_LKAS_ANGLE_RX_CHECKS(SUBARU_ALT_BUS) + }; + const uint16_t SUBARU_PARAM_GEN2 = 1; subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2); #ifdef ALLOW_DEBUG const uint16_t SUBARU_PARAM_LONGITUDINAL = 2; subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL); + + const uint16_t SUBARU_PARAM_LKAS_ANGLE = 8; + subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE); #endif safety_config ret; - if (subaru_gen2) { + if (subaru_lkas_angle) { + ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_lkas_angle_rx_checks, SUBARU_LKAS_ANGLE_LONG_TX_MSGS) : \ + BUILD_SAFETY_CFG(subaru_lkas_angle_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/safety_replay/helpers.py b/opendbc/safety/tests/safety_replay/helpers.py index 4f91a9bf68c..0a55354e911 100644 --- a/opendbc/safety/tests/safety_replay/helpers.py +++ b/opendbc/safety/tests/safety_replay/helpers.py @@ -1,5 +1,6 @@ from opendbc.car.ford.values import FordSafetyFlags from opendbc.car.hyundai.values import HyundaiSafetyFlags +from opendbc.car.subaru.values import SubaruSafetyFlags from opendbc.car.toyota.values import ToyotaSafetyFlags from opendbc.car.structs import CarParams from opendbc.safety.tests.libsafety import libsafety_py @@ -29,7 +30,7 @@ def is_steering_msg(mode, param, addr): elif mode == CarParams.SafetyModel.chrysler: ret = addr == 0x292 elif mode == CarParams.SafetyModel.subaru: - ret = addr == 0x122 + ret = addr == 0x122 or addr == 0x124 elif mode == CarParams.SafetyModel.ford: ret = addr == 0x3d6 if param & FordSafetyFlags.CANFD else addr == 0x3d3 elif mode == CarParams.SafetyModel.nissan: @@ -64,8 +65,11 @@ def get_steer_value(mode, param, msg): elif mode == CarParams.SafetyModel.chrysler: torque = (((msg.data[0] & 0x7) << 8) | msg.data[1]) - 1024 elif mode == CarParams.SafetyModel.subaru: - torque = ((msg.data[3] & 0x1F) << 8) | msg.data[2] - torque = -to_signed(torque, 13) + if param & SubaruSafetyFlags.LKAS_ANGLE: + angle = -to_signed(((msg.data[5] | (msg.data[6] << 8) | (msg.data[7] << 16)) & 0x1FFFF), 17) + else: + torque = ((msg.data[3] & 0x1F) << 8) | msg.data[2] + torque = -to_signed(torque, 13) elif mode == CarParams.SafetyModel.ford: if param & FordSafetyFlags.CANFD: angle = ((msg.data[2] << 3) | (msg.data[3] >> 5)) - 1000 diff --git a/opendbc/safety/tests/test_subaru.py b/opendbc/safety/tests/test_subaru.py index d5183eecd4c..29242b0e17a 100755 --- a/opendbc/safety/tests/test_subaru.py +++ b/opendbc/safety/tests/test_subaru.py @@ -168,6 +168,43 @@ def _torque_cmd_msg(self, torque, steer_req=1): return self.packer.make_can_msg_safety("ES_LKAS", SUBARU_MAIN_BUS, values) +class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafetyTest): + ALT_MAIN_BUS = SUBARU_ALT_BUS + ALT_CAM_BUS = SUBARU_ALT_BUS + + TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS, SubaruMsg.ES_LKAS_ANGLE) + RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_DashStatus, + SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment)} + FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr(SubaruMsg.ES_LKAS_ANGLE) + + FLAGS = SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.GEN2 + + STEER_ANGLE_MAX = 545 + # Avoid overflow of ES_LKAS_ANGLE's 17-bit signed field (0.01 deg resolution): limit test angles + STEER_ANGLE_TEST_MAX = 545 + ANGLE_RATE_BP = [0, 5, 35] + ANGLE_RATE_UP = [5, 0.8, 0.15] + ANGLE_RATE_DOWN = [5, 0.8, 0.15] + + def _angle_cmd_msg(self, angle, enabled=1): + values = {"LKAS_Output": angle, "LKAS_Request": enabled, "SET_3": 3} + return self.packer.make_can_msg_safety("ES_LKAS_ANGLE", SUBARU_MAIN_BUS, values) + + def _angle_meas_msg(self, angle): + values = {"Steering_Angle": angle} + return self.packer.make_can_msg_safety("Steering_2", SUBARU_MAIN_BUS, values) + + def _speed_msg(self, speed): + # convert meters-per-second to kilometers per hour for message + values = {s: speed * 3.6 for s in ["FR", "FL", "RR", "RL"]} + return self.packer.make_can_msg_safety("Wheel_Speeds", self.ALT_MAIN_BUS, values) + + # need to use ES_Brake Message + def _pcm_status_msg(self, enable): + values = {"Cruise_Activated": enable} + return self.packer.make_can_msg_safety("ES_Brake", self.ALT_CAM_BUS, values) + + class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase): FLAGS = 0 TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) @@ -195,6 +232,11 @@ class TestSubaruGen1LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSub SubaruMsg.ES_Distance)} +class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase): + ALT_MAIN_BUS = SUBARU_ALT_BUS + FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE + + class TestSubaruGen2LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase): FLAGS = SubaruSafetyFlags.LONG | SubaruSafetyFlags.GEN2 TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS) + long_tx_msgs(SUBARU_ALT_BUS) + gen2_long_additional_tx_msgs()