diff --git a/.gitignore b/.gitignore index 5a595d00afa..9de89119a25 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +.idea/ .cache/ /build/ .mypy_cache/ diff --git a/docs/CARS.md b/docs/CARS.md index 60f5f600eee..e32a6d1b503 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -1,6 +1,6 @@ -# Support Information for 383 Known Cars +# Support Information for 384 Known Cars |Make|Model|Package|Support Level| |---|---|---|:---:| @@ -259,6 +259,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|[Upstream](#upstream)| |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/fingerprints.py b/opendbc/car/fingerprints.py index 206177f8ce7..c0ea3e2169b 100644 --- a/opendbc/car/fingerprints.py +++ b/opendbc/car/fingerprints.py @@ -259,6 +259,7 @@ def all_legacy_fingerprint_cars(): "SUBARU IMPREZA LIMITED 2019": SUBARU.SUBARU_IMPREZA, "SUBARU IMPREZA SPORT 2020": SUBARU.SUBARU_IMPREZA_2020, "SUBARU CROSSTREK HYBRID 2020": SUBARU.SUBARU_CROSSTREK_HYBRID, + "SUBARU CROSSTREK 2025": SUBARU.SUBARU_CROSSTREK_2025, "SUBARU FORESTER 2019": SUBARU.SUBARU_FORESTER, "SUBARU FORESTER HYBRID 2020": SUBARU.SUBARU_FORESTER_HYBRID, "SUBARU FORESTER 2017 - 2018": SUBARU.SUBARU_FORESTER_PREGLOBAL, diff --git a/opendbc/car/subaru/carcontroller.py b/opendbc/car/subaru/carcontroller.py index 3b9abf5917b..5a52e10d81b 100644 --- a/opendbc/car/subaru/carcontroller.py +++ b/opendbc/car/subaru/carcontroller.py @@ -1,21 +1,22 @@ 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_steer_angle_limits_vm 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 +from opendbc.car.vehicle_model import VehicleModel # FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and # involves the total steering angle change rather than rate, but these limits work well for now MAX_STEER_RATE = 25 # deg/s MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut - class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) self.apply_torque_last = 0 + self.apply_steer_last = 0 self.cruise_button_prev = 0 self.steer_rate_counter = 0 @@ -23,6 +24,9 @@ def __init__(self, dbc_names, CP): self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + # Vehicle model used for lateral limiting + self.VM = VehicleModel(CP) + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -32,30 +36,48 @@ 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 + if self.CP.flags & SubaruFlags.LKAS_ANGLE: + actual_steering_angle_deg = CS.out.steeringAngleDeg + desired_steering_angle_deg = actuators.steeringAngleDeg + + apply_steer = apply_steer_angle_limits_vm( + desired_steering_angle_deg, + self.apply_steer_last, + CS.out.vEgoRaw, + actual_steering_angle_deg, + CC.latActive, + CarControllerParams, + self.VM + ) + + if not CC.latActive: + apply_steer = actual_steering_angle_deg + + can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive)) + self.apply_steer_last = apply_steer + else: + apply_torque = int(round(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) + 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 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)) - else: - apply_steer_req = CC.latActive + 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)) + 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) + 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)) + can_sends.append(subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req)) - self.apply_torque_last = apply_torque + self.apply_torque_last = apply_torque # *** longitudinal *** @@ -137,8 +159,11 @@ def update(self, CC, CS, now_nanos): can_sends.append(subarucan.create_es_static_2(self.packer)) new_actuators = actuators.as_builder() - new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX - new_actuators.torqueOutputCan = self.apply_torque_last + if self.CP.flags & SubaruFlags.LKAS_ANGLE: + new_actuators.steeringAngleDeg = self.apply_steer_last + else: + new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX + new_actuators.torqueOutputCan = self.apply_torque_last self.frame += 1 return new_actuators, can_sends diff --git a/opendbc/car/subaru/carstate.py b/opendbc/car/subaru/carstate.py index 76bc4ff8f77..36698b74854 100644 --- a/opendbc/car/subaru/carstate.py +++ b/opendbc/car/subaru/carstate.py @@ -60,11 +60,17 @@ def update(self, can_parsers) -> structs.CarState: can_gear = int(cp_transmission.vl["Transmission"]["Gear"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) - ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"] + if self.CP.flags & SubaruFlags.LKAS_ANGLE: + ret.steeringAngleDeg = cp.vl["Steering_2"]["Steering_Angle"] + counter = cp.vl["Steering_2"]["COUNTER"] - if not (self.CP.flags & SubaruFlags.PREGLOBAL): - # ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it - ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, cp.vl["Steering_Torque"]["COUNTER"]) + ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, counter) + else: + ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"] + + if not (self.CP.flags & SubaruFlags.PREGLOBAL): + # ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it + ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, cp.vl["Steering_Torque"]["COUNTER"]) ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"] ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"] @@ -73,7 +79,7 @@ def update(self, can_parsers) -> structs.CarState: ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold cp_cruise = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp - if self.CP.flags & SubaruFlags.HYBRID: + if self.CP.flags & SubaruFlags.HYBRID or self.CP.flags & SubaruFlags.LKAS_ANGLE: ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0 ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0 else: diff --git a/opendbc/car/subaru/fingerprints.py b/opendbc/car/subaru/fingerprints.py index 64158446bd3..72a72c84292 100644 --- a/opendbc/car/subaru/fingerprints.py +++ b/opendbc/car/subaru/fingerprints.py @@ -243,6 +243,20 @@ b'\xd7!`p\x07', b'\xf4!`0\x07', ], + }, + CAR.SUBARU_CROSSTREK_2025: { + (Ecu.abs, 0x7b0, None): [ + b'\xa2 $\x17\x06', + ], + (Ecu.eps, 0x746, None): [ + b'\xc2 $\x00\x01', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x1d!\x08\x00F\x14!\x08\x00=', + ], + (Ecu.engine, 0x7a2, None): [ + b'\x04"cP\x07', + ], }, CAR.SUBARU_FORESTER: { (Ecu.abs, 0x7b0, None): [ diff --git a/opendbc/car/subaru/interface.py b/opendbc/car/subaru/interface.py index b11a987d55d..a940a0a5856 100644 --- a/opendbc/car/subaru/interface.py +++ b/opendbc/car/subaru/interface.py @@ -42,6 +42,9 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, alp else: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + if ret.flags & SubaruFlags.LKAS_ANGLE: + ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LKAS_ANGLE.value + if candidate in (CAR.SUBARU_ASCENT, CAR.SUBARU_ASCENT_2023): ret.steerActuatorDelay = 0.3 # end-to-end angle controller ret.lateralTuning.init('pid') @@ -65,6 +68,10 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, alp elif candidate == CAR.SUBARU_CROSSTREK_HYBRID: ret.steerActuatorDelay = 0.1 + elif candidate == CAR.SUBARU_CROSSTREK_2025: + ret.dashcamOnly = False + ret.steerActuatorDelay = 0.3 + elif candidate in (CAR.SUBARU_FORESTER, CAR.SUBARU_FORESTER_2022, CAR.SUBARU_FORESTER_HYBRID): ret.lateralTuning.init('pid') ret.lateralTuning.pid.kf = 0.000038 diff --git a/opendbc/car/subaru/values.py b/opendbc/car/subaru/values.py index a702d39430f..c9d97f9074f 100644 --- a/opendbc/car/subaru/values.py +++ b/opendbc/car/subaru/values.py @@ -1,7 +1,8 @@ from dataclasses import dataclass, field from enum import Enum, IntFlag -from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds +from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds, ACCELERATION_DUE_TO_GRAVITY +from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL 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 @@ -52,11 +53,30 @@ def __init__(self, CP): BRAKE_LOOKUP_BP = [-3.5, 0] BRAKE_LOOKUP_V = [BRAKE_MAX, BRAKE_MIN] + AVERAGE_ROAD_ROLL = 0.06 # ~3.4 degrees, 6% superelevation. + + ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits( + 100, + # v1 limits, not used + ([], []), + ([], []), + + # Vehicle model angle limits + # Add extra tolerance for average banked road since safety doesn't have the roll + MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), # ~3.6 m/s^2 + MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), # ~3.6 m/s^3 + + # limit angle rate to both prevent a fault and for low speed comfort (~12 mph rate down to 0 mph) + # FIXME: convert from carcontroller known limit + # MAX_STEER_RATE = 25 # deg/s + MAX_ANGLE_RATE=5, # deg/20ms frame + ) class SubaruSafetyFlags(IntFlag): GEN2 = 1 LONG = 2 PREGLOBAL_REVERSED_DRIVER_TORQUE = 4 + LKAS_ANGLE = 8 class SubaruFlags(IntFlag): @@ -211,6 +231,11 @@ class CAR(Platforms): SUBARU_ASCENT.specs, flags=SubaruFlags.LKAS_ANGLE, ) + SUBARU_CROSSTREK_2025 = SubaruGen2PlatformConfig( + [SubaruCarDocs("Subaru Crosstrek 2025", "All", car_parts=CarParts.common([CarHarness.subaru_d]))], + CarSpecs(mass=1529, wheelbase=2.5781, steerRatio=13.5), + flags=SubaruFlags.LKAS_ANGLE + ) SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ diff --git a/opendbc/car/tests/routes.py b/opendbc/car/tests/routes.py index 6e98dea40b9..6d91d309d31 100644 --- a/opendbc/car/tests/routes.py +++ b/opendbc/car/tests/routes.py @@ -299,6 +299,7 @@ class CarTestRoute(NamedTuple): CarTestRoute("1bbe6bf2d62f58a8/2022-07-14--17-11-43", SUBARU.SUBARU_OUTBACK, segment=10), CarTestRoute("c56e69bbc74b8fad/2022-08-18--09-43-51", SUBARU.SUBARU_LEGACY, segment=3), CarTestRoute("f4e3a0c511a076f4/2022-08-04--16-16-48", SUBARU.SUBARU_CROSSTREK_HYBRID, segment=2), + CarTestRoute("38b065e31c0a9ed7/00000001--5656806473", SUBARU.SUBARU_CROSSTREK_2025, segment=2), CarTestRoute("7fd1e4f3a33c1673/2022-12-04--15-09-53", SUBARU.SUBARU_FORESTER_2022, segment=4), CarTestRoute("f3b34c0d2632aa83/2023-07-23--20-43-25", SUBARU.SUBARU_OUTBACK_2023, segment=7), CarTestRoute("99437cef6d5ff2ee/2023-03-13--21-21-38", SUBARU.SUBARU_ASCENT_2023, segment=7), diff --git a/opendbc/car/torque_data/override.toml b/opendbc/car/torque_data/override.toml index bcd99a23a54..6cee48bc5c6 100644 --- a/opendbc/car/torque_data/override.toml +++ b/opendbc/car/torque_data/override.toml @@ -14,6 +14,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "SUBARU_FORESTER_2022" = [nan, 3.0, nan] "SUBARU_OUTBACK_2023" = [nan, 3.0, nan] "SUBARU_ASCENT_2023" = [nan, 3.0, nan] +"SUBARU_CROSSTREK_2025" = [nan, 3.0, nan] # Toyota LTA also has torque "TOYOTA_RAV4_TSS2_2023" = [nan, 3.0, nan] diff --git a/opendbc/safety/modes/subaru.h b/opendbc/safety/modes/subaru.h index 9b01d39d635..b32298cb3d7 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,17 @@ {.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_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]; @@ -91,24 +102,38 @@ 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 ((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); - torque_driver_new = -1 * to_signed(torque_driver_new, 11); - update_sample(&torque_driver, torque_driver_new); + const unsigned int alt_main_bus = (subaru_gen2 || subaru_lkas_angle) ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; - int angle_meas_new = (GET_BYTES(msg, 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); + if (subaru_lkas_angle && (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 = ROUND(to_signed(raw, 17) * -1); update_sample(&angle_meas, angle_meas_new); + } else { + 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); + torque_driver_new = -1 * to_signed(torque_driver_new, 11); + update_sample(&torque_driver, torque_driver_new); + + int angle_meas_new = (GET_BYTES(msg, 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); + update_sample(&angle_meas, angle_meas_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_DashStatus) && (msg->bus == SUBARU_CAM_BUS)) { + bool cruise_engaged = (msg->data[4] >> 4) & 1U; + pcm_cruise_check(cruise_engaged); + } + } else { + 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 @@ -136,6 +161,22 @@ 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., + // FIXME: max_angle_rate from tesla + .frequency = 50U, + }; + + // CarSpecs values for 2025 Crosstrek + // FIXME: add values for all 4 supported angle based models + const AngleSteeringParams SUBARU_ANGLE_STEERING_PARAMS = { + // FIXME: tesla value, find out how to calculate for 2025 Crosstrek + .slip_factor = -0.000580374383851451, // calc_slip_factor(VM) + .steer_ratio = 13.5, + .wheelbase = 2.5781, + }; + 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 @@ -160,6 +201,14 @@ static bool subaru_tx_hook(const CANPacket_t *msg) { violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits); } + 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_vm(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS, SUBARU_ANGLE_STEERING_PARAMS); + } + // check es_brake brake_pressure limits if (msg->addr == MSG_SUBARU_ES_Brake) { int es_brake_pressure = GET_BYTES(msg, 2, 2); @@ -225,6 +274,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) }; @@ -233,9 +294,15 @@ static safety_config subaru_init(uint16_t param) { SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS) }; + static RxCheck subaru_lkas_angle_rx_checks[] = { + SUBARU_LKAS_ANGLE_RX_CHECKS(SUBARU_ALT_BUS) + }; + const uint16_t SUBARU_PARAM_GEN2 = 1; + 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; @@ -243,7 +310,10 @@ static safety_config subaru_init(uint16_t param) { #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 a0ad42d8d67..25b0899177b 100755 --- a/opendbc/safety/tests/test_subaru.py +++ b/opendbc/safety/tests/test_subaru.py @@ -1,12 +1,16 @@ #!/usr/bin/env python3 import enum import unittest +import numpy as np -from opendbc.car.subaru.values import SubaruSafetyFlags +from opendbc.car.lateral import get_max_angle_delta_vm, get_max_angle_vm +from opendbc.car.subaru.values import SubaruSafetyFlags, CarControllerParams, CAR +from opendbc.car.subaru.interface import CarInterface from opendbc.car.structs import CarParams +from opendbc.car.vehicle_model import VehicleModel from opendbc.safety.tests.libsafety import libsafety_py import opendbc.safety.tests.common as common -from opendbc.safety.tests.common import CANPackerSafety +from opendbc.safety.tests.common import CANPackerSafety, away_round, round_speed from functools import partial @@ -58,6 +62,11 @@ def gen2_long_additional_tx_msgs(): def fwd_blacklisted_addr(lkas_msg=SubaruMsg.ES_LKAS): return {SUBARU_CAM_BUS: [lkas_msg, SubaruMsg.ES_DashStatus, SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment]} +def round_angle(apply_angle, can_offset=0): + apply_angle_can = (apply_angle + 1638.35) / 0.1 + can_offset + # 0.49999_ == 0.5 + rnd_offset = 1e-5 if apply_angle >= 0 else -1e-5 + return away_round(apply_angle_can + rnd_offset) * 0.1 - 1638.35 class TestSubaruSafetyBase(common.CarSafetyTest): FLAGS = 0 @@ -77,7 +86,13 @@ class TestSubaruSafetyBase(common.CarSafetyTest): INACTIVE_GAS = 1818 + def _get_steer_cmd_angle_max(self, speed): + return get_max_angle_vm(max(speed, 1), self.VM, CarControllerParams) + def setUp(self): + # FIXME: move to Angle class + CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025) + self.VM = VehicleModel(CP) self.packer = CANPackerSafety("subaru_global_2017_generated") self.safety = libsafety_py.libsafety self.safety.set_safety_hooks(CarParams.SafetyModel.subaru, self.FLAGS) @@ -172,6 +187,110 @@ 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 + + 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, 15, 15] + ANGLE_RATE_UP = [5, 0.15, 0.15] + ANGLE_RATE_DOWN = [5, 0.4, 0.4] + + 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_DashStatus Message + def _pcm_status_msg(self, enable): + values = {"Cruise_Activated": enable} + return self.packer.make_can_msg_safety("ES_DashStatus", self.ALT_CAM_BUS, values) + + # FIXME: copy-paste from tesla + def test_lateral_accel_limit(self): + for speed in np.linspace(0, 40, 100): + speed = max(speed, 1) + # match DI_vehicleSpeed rounding on CAN + speed = round_speed(away_round(speed / 0.08 * 3.6) * 0.08 / 3.6) + for sign in (-1, 1): + self.safety.set_controls_allowed(True) + self._reset_speed_measurement(speed + 1) # safety fudges the speed + + # angle signal can't represent 0, so it biases one unit down + angle_unit_offset = -1 if sign == -1 else 0 + + # at limit (safety tolerance adds 1) + max_angle = round_angle(get_max_angle_vm(speed, self.VM, CarControllerParams), angle_unit_offset + 1) * sign + max_angle = np.clip(max_angle, -self.STEER_ANGLE_MAX, self.STEER_ANGLE_MAX) + self.safety.set_desired_angle_last(round(max_angle * self.DEG_TO_CAN)) + + self.assertTrue(self._tx(self._angle_cmd_msg(max_angle, True))) + + # 1 unit above limit + max_angle_raw = round_angle(get_max_angle_vm(speed, self.VM, CarControllerParams), angle_unit_offset + 2) * sign + max_angle = np.clip(max_angle_raw, -self.STEER_ANGLE_MAX, self.STEER_ANGLE_MAX) + self._tx(self._angle_cmd_msg(max_angle, True)) + + # at low speeds max angle is above 360, so adding 1 has no effect + should_tx = abs(max_angle_raw) >= self.STEER_ANGLE_MAX + self.assertEqual(should_tx, self._tx(self._angle_cmd_msg(max_angle, True))) + + # FIXME: copy-paste from tesla + def test_lateral_jerk_limit(self): + for speed in np.linspace(0, 40, 100): + speed = max(speed, 1) + # match DI_vehicleSpeed rounding on CAN + speed = round_speed(away_round(speed / 0.08 * 3.6) * 0.08 / 3.6) + for sign in (-1, 1): # (-1, 1): + self.safety.set_controls_allowed(True) + self._reset_speed_measurement(speed + 1) # safety fudges the speed + self._tx(self._angle_cmd_msg(0, True)) + + # angle signal can't represent 0, so it biases one unit down + angle_unit_offset = 1 if sign == -1 else 0 + + # Stay within limits + # Up + max_angle_delta = round_angle(get_max_angle_delta_vm(speed, self.VM, CarControllerParams), angle_unit_offset) * sign + self.assertTrue(self._tx(self._angle_cmd_msg(max_angle_delta, True))) + + # Don't change + self.assertTrue(self._tx(self._angle_cmd_msg(max_angle_delta, True))) + + # Down + self.assertTrue(self._tx(self._angle_cmd_msg(0, True))) + + # Inject too high rates + # Up + max_angle_delta = round_angle(get_max_angle_delta_vm(speed, self.VM, CarControllerParams), angle_unit_offset + 1) * sign + self.assertFalse(self._tx(self._angle_cmd_msg(max_angle_delta, True))) + + # Don't change + self.safety.set_desired_angle_last(round(max_angle_delta * self.DEG_TO_CAN)) + self.assertTrue(self._tx(self._angle_cmd_msg(max_angle_delta, True))) + + # Down + self.assertFalse(self._tx(self._angle_cmd_msg(0, True))) + + # Recover + self.assertTrue(self._tx(self._angle_cmd_msg(0, True))) + class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase): FLAGS = 0 TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) @@ -198,6 +317,9 @@ class TestSubaruGen1LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSub SubaruMsg.ES_Infotainment, SubaruMsg.ES_Brake, SubaruMsg.ES_Status, 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