diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 3b9abf5917..d6769489ce 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -1,21 +1,32 @@ 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 import Bus, make_tester_present_msg, structs +from opendbc.car.lateral import (apply_driver_steer_torque_limits, apply_std_steer_angle_limits, + apply_steer_angle_limits_vm, common_fault_avoidance) 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.subaru.values import CAR, 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 +GEN2_ANGLE_PORTS = frozenset((CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023)) 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.angle_was_active = False + self.angle_override_frames = 0 + self.ascent_angle_initialized = False + self.ascent_lkas_alert_counter_last = None + self.angle_driver_override = False + self.angle_handoff_active = False + self.VM = VehicleModel(CP) if CP.carFingerprint in GEN2_ANGLE_PORTS else None self.cruise_button_prev = 0 self.steer_rate_counter = 0 @@ -23,6 +34,46 @@ class CarController(CarControllerBase): self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + def angle_active(self, CS, available): + if not available: + self.angle_driver_override = False + self.angle_override_frames = 0 + self.angle_handoff_active = False + return False + high, low = (150, 100) if self.CP.carFingerprint == CAR.SUBARU_CROSSTREK_2025 else (200, 150) + torque = abs(CS.out.steeringTorque) + if self.angle_driver_override: + if torque < low: + self.angle_driver_override = False + elif torque > high: + self.angle_override_frames += 1 + if self.angle_override_frames >= 2: + self.angle_driver_override = True + self.angle_override_frames = 0 + else: + self.angle_override_frames = 0 + if self.angle_driver_override: + self.angle_handoff_active = True + return False + if self.angle_handoff_active: + if abs(CS.out.steeringRateDeg) <= 2.0: + self.angle_handoff_active = False + return False + if not self.angle_was_active and abs(CS.out.steeringRateDeg) > 2.0: + self.angle_handoff_active = True + return False + return True + + def _ascent_lkas_alert(self, CC, CS): + if self.CP.carFingerprint != CAR.SUBARU_ASCENT_2023 or not self.CP.flags & SubaruFlags.ASCENT_LKAS_ALERT: + return None + dat = CS.es_lkas_alert_dat + msg = subarucan.create_ascent_lkas_alert(dat, self.angle_was_active and CC.enabled and CC.latActive, CC.hudControl.visualAlert) + if msg is None or (dat[1] & 0xf) == self.ascent_lkas_alert_counter_last: + return None + self.ascent_lkas_alert_counter_last = dat[1] & 0xf + return msg + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -32,30 +83,56 @@ class CarController(CarControllerBase): # *** 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.carFingerprint in GEN2_ANGLE_PORTS: + angle_available = (CC.enabled and CC.latActive and CS.out.cruiseState.available and CS.out.cruiseState.enabled and + CS.out.gearShifter == structs.CarState.GearShifter.drive and + not CS.out.standstill and not CS.out.brakePressed) + angle_active = self.angle_active(CS, angle_available) + if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023 and not self.ascent_angle_initialized: + self.apply_angle_last = CS.out.steeringAngleDeg + self.ascent_angle_initialized = True + angle_active = False + if angle_active and not self.angle_was_active and self.CP.carFingerprint != CAR.SUBARU_ASCENT_2023: + self.apply_angle_last = CS.out.steeringAngleDeg + if self.CP.carFingerprint in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023): + apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, + CS.out.vEgoRaw, CS.out.steeringAngleDeg, angle_active, + self.p.FIXED_ANGLE_LIMITS) + else: + apply_angle = apply_steer_angle_limits_vm(actuators.steeringAngleDeg, self.apply_angle_last, + CS.out.vEgoRaw, CS.out.steeringAngleDeg, angle_active, + self.p, self.VM) + if not angle_active and self.CP.carFingerprint != CAR.SUBARU_ASCENT_2023: + apply_angle = CS.out.steeringAngleDeg + self.apply_angle_last = apply_angle + self.angle_was_active = angle_active + angle_bus = CanBus.main if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023 else CanBus.alt + can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_angle, angle_active, angle_bus)) else: - apply_steer_req = CC.latActive + apply_torque = int(round(actuators.torque * self.p.STEER_MAX)) - 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) + # limits due to driver torque - can_sends.append(subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req)) + new_torque = int(round(apply_torque)) + apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.p) - self.apply_torque_last = apply_torque + 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.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 # *** longitudinal *** @@ -74,6 +151,9 @@ class CarController(CarControllerBase): cruise_brake = CarControllerParams.BRAKE_MIN # *** alerts and pcm cancel *** + lkas_alert_msg = self._ascent_lkas_alert(CC, CS) + if lkas_alert_msg is not None: + can_sends.append(lkas_alert_msg) if self.CP.flags & SubaruFlags.PREGLOBAL: if self.frame % 5 == 0: # 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep @@ -139,6 +219,8 @@ class CarController(CarControllerBase): 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.carFingerprint in GEN2_ANGLE_PORTS: + new_actuators.steeringAngleDeg = self.apply_angle_last self.frame += 1 return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/car/subaru/carstate.py b/opendbc_repo/opendbc/car/subaru/carstate.py index 234aff6189..220a0d8c0a 100644 --- a/opendbc_repo/opendbc/car/subaru/carstate.py +++ b/opendbc_repo/opendbc/car/subaru/carstate.py @@ -3,7 +3,7 @@ from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.interfaces import CarStateBase -from opendbc.car.subaru.values import DBC, CanBus, SubaruFlags +from opendbc.car.subaru.values import CAR, DBC, CanBus, SubaruFlags from opendbc.car import CanSignalRateCalculator @@ -13,6 +13,7 @@ class CarState(CarStateBase): can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) self.shifter_values = can_define.dv["Transmission"]["Gear"] + self.es_lkas_alert_dat = b"" self.angle_rate_calulator = CanSignalRateCalculator(50) def update(self, can_parsers) -> structs.CarState: @@ -83,7 +84,10 @@ class CarState(CarStateBase): cp_cruise = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp cp_es_brake = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp_cam - if self.CP.flags & (SubaruFlags.HYBRID | SubaruFlags.LKAS_ANGLE): + if self.CP.carFingerprint in (CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023): + ret.cruiseState.enabled = cp_es_brake.vl["ES_Status"]["Cruise_Activated"] != 0 + ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]["Cruise_On"] != 0 + elif self.CP.flags & (SubaruFlags.HYBRID | SubaruFlags.LKAS_ANGLE): # ES_DashStatus->Cruise_Activated_Dash is likely intended for the dash display only, as it falls # during user gas override and at standstill. ES_Status is missing on hybrid, so we use ES_Brake instead @@ -134,6 +138,10 @@ class CarState(CarStateBase): self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"]) + if self.CP.flags & SubaruFlags.ASCENT_LKAS_ALERT: + alert = cp_cam.vl["ES_LKAS_Alert"] + self.es_lkas_alert_dat = (bytes((int(alert["CHECKSUM"]), int(alert["COUNTER"]) | (int(alert["Signal1"]) << 4))) + + int(alert["Payload"]).to_bytes(6, "little")) if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"]) @@ -143,6 +151,6 @@ class CarState(CarStateBase): def get_can_parsers(CP): return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main), - Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.camera), + Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_Alert", 0)] if CP.flags & SubaruFlags.ASCENT_LKAS_ALERT else [], CanBus.camera), Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt) } diff --git a/opendbc_repo/opendbc/car/subaru/interface.py b/opendbc_repo/opendbc/car/subaru/interface.py index 1a8260f873..156f6bf6a4 100644 --- a/opendbc_repo/opendbc/car/subaru/interface.py +++ b/opendbc_repo/opendbc/car/subaru/interface.py @@ -3,7 +3,9 @@ from opendbc.car.disable_ecu import disable_ecu from opendbc.car.interfaces import CarInterfaceBase from opendbc.car.subaru.carcontroller import CarController from opendbc.car.subaru.carstate import CarState -from opendbc.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags, SubaruSafetyFlags +from opendbc.car.subaru.values import CAR, GLOBAL_ES_ADDR, CanBus, SubaruFlags, SubaruSafetyFlags + +GEN2_ANGLE_PORTS = frozenset((CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023)) class CarInterface(CarInterfaceBase): @@ -18,13 +20,17 @@ class CarInterface(CarInterfaceBase): # - 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)) + ret.dashcamOnly = bool(ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.HYBRID) or + (ret.flags & SubaruFlags.LKAS_ANGLE and candidate not in GEN2_ANGLE_PORTS)) ret.autoResumeSng = False # Detect infotainment message sent from the camera if not (ret.flags & SubaruFlags.PREGLOBAL) and 0x323 in fingerprint[2]: ret.flags |= SubaruFlags.SEND_INFOTAINMENT.value + if candidate == CAR.SUBARU_ASCENT_2023 and fingerprint[CanBus.camera].get(0x3C4) == 8: + ret.flags |= SubaruFlags.ASCENT_LKAS_ALERT.value + if ret.flags & SubaruFlags.PREGLOBAL: if 0x25c in fingerprint[0]: ret.flags |= SubaruFlags.HAS_BSM.value @@ -35,6 +41,14 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.subaru)] if ret.flags & SubaruFlags.GLOBAL_GEN2: ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.GEN2.value + if candidate in GEN2_ANGLE_PORTS: + ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LKAS_ANGLE.value + if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023): + ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value + if candidate == CAR.SUBARU_ASCENT_2023: + ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.ANGLE_MAIN_BUS.value + if ret.flags & SubaruFlags.ASCENT_LKAS_ALERT: + ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.ASCENT_LKAS_ALERT.value ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 diff --git a/opendbc_repo/opendbc/car/subaru/subarucan.py b/opendbc_repo/opendbc/car/subaru/subarucan.py index 8f824a5849..6c31a9b8a6 100644 --- a/opendbc_repo/opendbc/car/subaru/subarucan.py +++ b/opendbc_repo/opendbc/car/subaru/subarucan.py @@ -13,13 +13,13 @@ def create_steering_control(packer, apply_torque, steer_req): return packer.make_can_msg("ES_LKAS", 0, values) -def create_steering_control_angle(packer, apply_torque, steer_req): +def create_steering_control_angle(packer, apply_torque, steer_req, bus=CanBus.main): values = { "LKAS_Output": apply_torque, "LKAS_Request": steer_req, "SET_3": 3 } - return packer.make_can_msg("ES_LKAS_ANGLE", 0, values) + return packer.make_can_msg("ES_LKAS_ANGLE", bus, values) def create_steering_status(packer): @@ -257,6 +257,18 @@ def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert): return packer.make_can_msg("ES_Infotainment", CanBus.main, values) +def create_ascent_lkas_alert(raw_dat: bytes, lateral_active: bool, visual_alert): + address = 0x3C4 + if len(raw_dat) != 8 or raw_dat[0] != (((address & 0xff) + (address >> 8) + sum(raw_dat[1:])) & 0xff): + return None + + dat = bytearray(raw_dat) + if lateral_active and visual_alert != VisualAlert.steerRequired and dat[2:] == b"\x00\x01\x04\x00\x00\x00": + dat[3:5] = b"\x00\x00" + dat[0] = ((address & 0xff) + (address >> 8) + sum(dat[1:])) & 0xff + return address, bytes(dat), CanBus.main + + def create_es_highbeamassist(packer): values = { "HBA_Available": False, diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_angle_handoff.py b/opendbc_repo/opendbc/car/subaru/tests/test_angle_handoff.py new file mode 100644 index 0000000000..f8fa2f5dda --- /dev/null +++ b/opendbc_repo/opendbc/car/subaru/tests/test_angle_handoff.py @@ -0,0 +1,53 @@ +import unittest + +from opendbc.car.subaru.tests.test_ascent_angle import setup_controller, step +from opendbc.car.subaru.values import CAR + + +class TestAngleHandoff(unittest.TestCase): + def test_override_release_waits_for_low_torque_and_settled_wheel(self): + for car, high, low in ((CAR.SUBARU_CROSSTREK_2025, 150, 100), (CAR.SUBARU_LEGACY_2025, 200, 150)): + for sign in (-1, 1): + with self.subTest(car=car, sign=sign): + _, controller, command, state = setup_controller(car) + state.out.steeringRateDeg = 0 + state.out.steeringAngleDeg = 10 + # One sample above the threshold must not hand control back and forth. + for torque, rate, expected in ((high, 0, 1), (high + 1, 0, 1), (high + 1, 0, 0), + (high, 0, 0), (low, 0, 0), (low - 1, 3, 0), + (0, 2, 0), (0, 2, 1)): + state.out.steeringTorque = torque * sign + state.out.steeringRateDeg = rate * sign + steer, decoded = step(controller, command, state) + self.assertEqual(steer[2], 1) + self.assertEqual(decoded['LKAS_Request'], expected) + if not expected: + self.assertEqual(decoded['LKAS_Output'], 10) + + def test_engagement_waits_for_manual_turn_to_settle(self): + for car in (CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025): + with self.subTest(car=car): + _, controller, command, state = setup_controller(car) + state.out.steeringTorque = 0 + for rate, expected in ((3, 0), (2.1, 0), (2, 0), (2, 1)): + state.out.steeringRateDeg = rate + _, decoded = step(controller, command, state) + self.assertEqual(decoded['LKAS_Request'], expected) + + def test_unavailable_control_clears_override_without_sending_active_commands(self): + for car in (CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025): + with self.subTest(car=car): + _, controller, command, state = setup_controller(car) + state.out.steeringRateDeg = 0 + state.out.steeringTorque = 300 + step(controller, command, state) + step(controller, command, state) + command.latActive = False + _, decoded = step(controller, command, state) + self.assertEqual(decoded['LKAS_Request'], 0) + self.assertFalse(controller.angle_driver_override) + self.assertFalse(controller.angle_handoff_active) + state.out.steeringTorque = 0 + command.latActive = True + _, decoded = step(controller, command, state) + self.assertEqual(decoded['LKAS_Request'], 1) diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_ascent_angle.py b/opendbc_repo/opendbc/car/subaru/tests/test_ascent_angle.py new file mode 100644 index 0000000000..cb6adebdd3 --- /dev/null +++ b/opendbc_repo/opendbc/car/subaru/tests/test_ascent_angle.py @@ -0,0 +1,143 @@ +import unittest +from math import isclose + +from opendbc.can import CANPacker, CANParser +from opendbc.car import Bus, structs +from opendbc.car.subaru import subarucan +from opendbc.car.subaru.carcontroller import CarController +from opendbc.car.subaru.carstate import CarState +from opendbc.car.subaru.interface import CarInterface +from opendbc.car.subaru.values import CAR, DBC, SubaruSafetyFlags +from opendbc.safety.tests import test_subaru_gen2_angle_pair as pair + + +CAR_ID = CAR.SUBARU_ASCENT_2023 + + +def setup_controller(car=CAR_ID): + cp = CarInterface.get_params(car, {0: {}, 1: {}, 2: {}}, [], False, False, False) + _, frames = pair.TestSubaruGen2AnglePair.sources(car) + state = CarState(cp) + parsers = state.get_can_parsers(cp) + state.update(parsers) + for parser in parsers.values(): + parser.update([(1_000_000_000, frames)]) + state.out = state.update(parsers) + command = structs.CarControl() + command.enabled = command.latActive = True + command.actuators.steeringAngleDeg = -1.45 + return cp, CarController(DBC[car], cp), command, state + + +def step(controller, command, state): + _, sends = controller.update(command.as_reader(), state, 1_000_000_000 + controller.frame * 10_000_000) + if controller.frame % 2 == 0: + _, sends = controller.update(command.as_reader(), state, 1_000_000_000 + controller.frame * 10_000_000) + steer = next(frame for frame in sends if frame[0] == 0x124) + parser = CANParser(DBC[controller.CP.carFingerprint][Bus.pt], [('ES_LKAS_ANGLE', 0)], steer[2]) + parser.update([(1, [steer])]) + return steer, parser.vl['ES_LKAS_ANGLE'] + + +class TestSubaruAscentController(unittest.TestCase): + def test_ascent_admission_and_outback_unchanged(self): + cp, _, _, state = setup_controller() + assert not cp.dashcamOnly + assert not cp.openpilotLongitudinalControl + assert not cp.alphaLongitudinalAvailable + assert cp.steerControlType == structs.CarParams.SteerControlType.angle + assert cp.safetyConfigs[0].safetyParam == (SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | + SubaruSafetyFlags.FIXED_ANGLE_LIMITS | SubaruSafetyFlags.ANGLE_MAIN_BUS) + assert state.out.cruiseState.enabled + assert state.out.cruiseState.available + outback = CarInterface.get_non_essential_params(CAR.SUBARU_OUTBACK_2023) + assert outback.dashcamOnly + assert outback.safetyConfigs[0].safetyParam == SubaruSafetyFlags.GEN2 + _, controller, command, state = setup_controller(CAR.SUBARU_OUTBACK_2023) + command.latActive = False + _, sends = controller.update(command.as_reader(), state, 1_000_000_000) + assert not any(frame[0] == 0x124 for frame in sends) + torque = next(frame for frame in sends if frame[0] == 0x122) + assert torque == subarucan.create_steering_control(CANPacker(DBC[CAR.SUBARU_OUTBACK_2023][Bus.pt]), 0, False) + + def test_first_entry_and_reentry_use_original_transmitted_history(self): + _, controller, command, state = setup_controller() + state.out.vEgoRaw = 30.3 + state.out.steeringAngleDeg = 0.78 + state.out.steeringRateDeg = -1.5 + controller.angle_handoff_active = True + steer, decoded = step(controller, command, state) + assert steer[2] == 0 + assert decoded['LKAS_Request'] == 0 + assert decoded['LKAS_Output'] == 0.78 + state.out.steeringAngleDeg = 0.74 + state.out.steeringRateDeg = -1.99 + _, decoded = step(controller, command, state) + assert decoded['LKAS_Request'] == 1 + assert isclose(decoded['LKAS_Output'], 0.53, abs_tol=0.01) + + _, initial, command, state = setup_controller() + state.out.vEgoRaw = 30.3 + state.out.steeringAngleDeg = 20.0 + state.out.steeringRateDeg = 0.0 + _, decoded = step(initial, command, state) + assert decoded['LKAS_Request'] == 0 + assert decoded['LKAS_Output'] == 20.0 + _, decoded = step(initial, command, state) + assert decoded['LKAS_Request'] == 1 + assert 19.7 < decoded['LKAS_Output'] < 20.0 + + def test_driver_override_hysteresis_and_settled_handoff(self): + _, controller, command, state = setup_controller() + state.out.vEgoRaw = 30.3 + state.out.steeringAngleDeg = -25.06 + state.out.steeringTorque = -250 + state.out.steeringRateDeg = 35 + for _ in range(2): + _, decoded = step(controller, command, state) + assert decoded['LKAS_Request'] == 0 + state.out.steeringTorque = -150 + state.out.steeringRateDeg = 0 + _, decoded = step(controller, command, state) + assert decoded['LKAS_Request'] == 0 + state.out.steeringTorque = -149 + state.out.steeringAngleDeg = -17.91 + _, decoded = step(controller, command, state) + assert decoded['LKAS_Request'] == 0 + assert decoded['LKAS_Output'] == -17.91 + _, decoded = step(controller, command, state) + assert decoded['LKAS_Request'] == 1 + assert -17.91 < decoded['LKAS_Output'] < -17.6 + + def test_ascent_inactive_follows_measurement_and_requires_stock_permission(self): + for revocation in ('brake', 'standstill', 'park', 'reverse', 'cruise', 'main', 'disabled', 'lateral'): + with self.subTest(revocation=revocation): + _, controller, command, state = setup_controller() + state.out.steeringAngleDeg = 12.5 + if revocation == 'brake': + state.out.brakePressed = True + elif revocation == 'standstill': + state.out.standstill = True + elif revocation in ('park', 'reverse'): + state.out.gearShifter = getattr(structs.CarState.GearShifter, revocation) + elif revocation == 'cruise': + state.out.cruiseState.enabled = False + elif revocation == 'main': + state.out.cruiseState.available = False + elif revocation == 'disabled': + command.enabled = False + else: + command.latActive = False + _, decoded = step(controller, command, state) + assert decoded['LKAS_Request'] == 0 + assert decoded['LKAS_Output'] == 12.5 + + def test_inactive_ascent_preserves_fixed_angle_bound(self): + for measured in (-600.0, 600.0): + with self.subTest(measured=measured): + _, controller, command, state = setup_controller() + state.out.steeringAngleDeg = measured + command.latActive = False + _, decoded = step(controller, command, state) + assert decoded['LKAS_Request'] == 0 + assert decoded['LKAS_Output'] == (545.0 if measured > 0 else -545.0) diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_ascent_lkas_alert.py b/opendbc_repo/opendbc/car/subaru/tests/test_ascent_lkas_alert.py new file mode 100644 index 0000000000..e86d7065be --- /dev/null +++ b/opendbc_repo/opendbc/car/subaru/tests/test_ascent_lkas_alert.py @@ -0,0 +1,68 @@ +import unittest + +from opendbc.car import structs +from opendbc.car.subaru.carcontroller import CarController +from opendbc.car.subaru.carstate import CarState +from opendbc.car.subaru.interface import CarInterface +from opendbc.car.subaru.values import CAR, DBC, SubaruFlags, SubaruSafetyFlags +from opendbc.car.subaru import subarucan +from opendbc.safety.tests.test_subaru_gen2_angle_pair import TestSubaruGen2AnglePair + + +class TestAscentLkasAlert(unittest.TestCase): + def params(self, car=CAR.SUBARU_ASCENT_2023, bus=2, length=8): + fingerprint = {0: {}, 1: {}, 2: {}} + fingerprint[bus][0x3C4] = length + return CarInterface.get_params(car, fingerprint, [], False, False, False) + + def test_platform_bus_length_gate_preserves_native_identity(self): + for car in CAR: + cp = self.params(car) + self.assertEqual(bool(cp.flags & SubaruFlags.ASCENT_LKAS_ALERT), car == CAR.SUBARU_ASCENT_2023) + self.assertEqual(bool(cp.safetyConfigs[0].safetyParam & SubaruSafetyFlags.ASCENT_LKAS_ALERT), car == CAR.SUBARU_ASCENT_2023) + for bus, length in ((0, 8), (1, 8), (2, 4), (2, 16)): + cp = self.params(bus=bus, length=length) + self.assertFalse(cp.flags & SubaruFlags.ASCENT_LKAS_ALERT) + self.assertEqual(cp.safetyConfigs[0].safetyParam, 0xB1) + self.assertEqual(self.params().safetyConfigs[0].safetyParam, 0x8B1) + + def test_production_parser_state_controller_and_actual_request(self): + cp = self.params() + state = CarState(cp) + parsers = state.get_can_parsers(cp) + state.update(parsers) + _, frames = TestSubaruGen2AnglePair.sources(CAR.SUBARU_ASCENT_2023) + raw = bytes.fromhex("ce02000104000000") + frames.append((0x3C4, raw, 2)) + for parser in parsers.values(): + parser.update([(1_000_000_000, frames)]) + state.out = state.update(parsers) + self.assertEqual(state.es_lkas_alert_dat, raw) + controller = CarController(DBC[cp.carFingerprint], cp) + cc = structs.CarControl() + cc.enabled = cc.latActive = True + _, sends = controller.update(cc.as_reader(), state, 1_000_000_000) + self.assertEqual(next(msg for msg in sends if msg[0] == 0x3C4)[1], raw) + self.assertIsNone(controller._ascent_lkas_alert(cc, state)) + controller.angle_was_active = True + for counter in (3, 15, 0): + dat = bytearray(raw) + dat[1] = counter | 0xA0 + dat[0] = (0xC4 + 3 + sum(dat[1:])) & 0xFF + state.es_lkas_alert_dat = bytes(dat) + msg = controller._ascent_lkas_alert(cc, state) + self.assertEqual(msg[1][1], dat[1]) + self.assertEqual(msg[1][2:], bytes(6)) + self.assertIsNone(controller._ascent_lkas_alert(cc, state)) + + def test_only_known_hands_on_pattern_masked(self): + raw = bytes.fromhex("ce02000104000000") + for active, alert, masked in ((True, 0, True), (False, 0, False), (True, structs.CarControl.HUDControl.VisualAlert.steerRequired, False)): + msg = subarucan.create_ascent_lkas_alert(raw, active, alert) + self.assertEqual(msg[1][2:], bytes(6) if masked else raw[2:]) + for payload in ("000000110000", "1e0030000000", "000704000000", "190000010000", "010104000000"): + dat = bytearray(bytes.fromhex("0002" + payload)) + dat[0] = (0xC4 + 3 + sum(dat[1:])) & 0xFF + self.assertEqual(subarucan.create_ascent_lkas_alert(bytes(dat), True, 0)[1], dat) + for dat in (b"", bytes(7), bytes(9), bytes.fromhex("0002000104000000")): + self.assertIsNone(subarucan.create_ascent_lkas_alert(dat, True, 0)) diff --git a/opendbc_repo/opendbc/car/subaru/values.py b/opendbc_repo/opendbc/car/subaru/values.py index 8a19121268..49a36e72a6 100644 --- a/opendbc_repo/opendbc/car/subaru/values.py +++ b/opendbc_repo/opendbc/car/subaru/values.py @@ -1,15 +1,28 @@ from dataclasses import dataclass, field from enum import Enum, IntFlag -from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds 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 +from opendbc.car.lateral import AngleSteeringLimits, AngleSteeringLimitsVM, ISO_LATERAL_ACCEL Ecu = CarParams.Ecu class CarControllerParams: + ANGLE_LIMITS = AngleSteeringLimitsVM( + 650, + MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + ACCELERATION_DUE_TO_GRAVITY * 0.06, + MAX_LATERAL_JERK=3.0 + ACCELERATION_DUE_TO_GRAVITY * 0.06, + MAX_ANGLE_RATE=1, + ) + FIXED_ANGLE_LIMITS = AngleSteeringLimits( + 545, ([0., 5., 35.], [5., .8, .15]), ([0., 5., 35.], [5., .8, .15]), + ) + + LEGACY_2025_ANGLE_LIMITS = FIXED_ANGLE_LIMITS + def __init__(self, CP): self.STEER_STEP = 2 # how often we update the steer cmd self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max @@ -57,6 +70,10 @@ class SubaruSafetyFlags(IntFlag): GEN2 = 1 LONG = 2 PREGLOBAL_REVERSED_DRIVER_TORQUE = 4 + LKAS_ANGLE = 16 + FIXED_ANGLE_LIMITS = 128 + ANGLE_MAIN_BUS = 32 + ASCENT_LKAS_ALERT = 2048 class SubaruFlags(IntFlag): @@ -74,6 +91,7 @@ class SubaruFlags(IntFlag): PREGLOBAL = 16 HYBRID = 32 LKAS_ANGLE = 64 + ASCENT_LKAS_ALERT = 512 GLOBAL_ES_ADDR = 0x787 @@ -212,6 +230,16 @@ class CAR(Platforms): SUBARU_ASCENT.specs, flags=SubaruFlags.LKAS_ANGLE, ) + SUBARU_LEGACY_2025 = SubaruGen2PlatformConfig( + [SubaruCarDocs("Subaru Legacy 2025", "All", car_parts=CarParts.common([CarHarness.subaru_d]))], + SUBARU_OUTBACK.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.67, steerRatio=17), + flags=SubaruFlags.LKAS_ANGLE, + ) SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ diff --git a/opendbc_repo/opendbc/dbc/generator/subaru/_subaru_global.dbc b/opendbc_repo/opendbc/dbc/generator/subaru/_subaru_global.dbc index 67f63216a5..d7ca0d1b2a 100644 --- a/opendbc_repo/opendbc/dbc/generator/subaru/_subaru_global.dbc +++ b/opendbc_repo/opendbc/dbc/generator/subaru/_subaru_global.dbc @@ -252,6 +252,12 @@ BO_ 803 ES_Infotainment: 8 XXX SG_ LKAS_State_Infotainment : 22|3@0+ (1,0) [0|7] "" XXX SG_ Signal2 : 24|1@0+ (1,0) [0|1] "" XXX +BO_ 964 ES_LKAS_Alert: 8 XXX + SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX + SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX + SG_ Signal1 : 12|4@1+ (1,0) [0|15] "" XXX + SG_ Payload : 16|48@1+ (1,0) [0|281474976710655] "" XXX + BO_ 722 AC_State: 8 XXX SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index 6e69af2155..9635d63f89 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -23,13 +23,17 @@ #define MSG_SUBARU_CruiseControl 0x240U #define MSG_SUBARU_Throttle 0x40U #define MSG_SUBARU_Steering_Torque 0x119U +#define MSG_SUBARU_Steering_2 0x11aU #define MSG_SUBARU_Wheel_Speeds 0x13aU #define MSG_SUBARU_ES_LKAS 0x122U +#define MSG_SUBARU_ES_LKAS_ANGLE 0x124U #define MSG_SUBARU_ES_Distance 0x221U +#define MSG_SUBARU_ES_Status 0x222U #define MSG_SUBARU_ES_DashStatus 0x321U #define MSG_SUBARU_ES_LKAS_State 0x322U #define MSG_SUBARU_ES_Infotainment 0x323U +#define MSG_SUBARU_ES_LKAS_Alert 0x3C4U #define SUBARU_MAIN_BUS 0U #define SUBARU_ALT_BUS 1U @@ -44,6 +48,13 @@ #define SUBARU_COMMON_TX_MSGS(alt_bus) \ {MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = false}, \ +#define SUBARU_GEN2_ANGLE_TX_MSGS(angle_bus) \ + {MSG_SUBARU_ES_LKAS_ANGLE, angle_bus, 8, .check_relay = true}, \ + {MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8, .check_relay = true}, \ + {MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8, .check_relay = true}, \ + {MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8, .check_relay = true}, \ + SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS) + #define SUBARU_COMMON_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 }}}, \ @@ -51,7 +62,20 @@ {.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_GEN2_ANGLE_RX_CHECKS() \ + {.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_Steering_2, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Wheel_Speeds, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Brake_Status, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_ES_Status, SUBARU_ALT_BUS, 8, 20U, .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 }}}, \ + static bool subaru_gen2 = false; +static bool subaru_lkas_angle = false; +static bool subaru_fixed_angle_limits = false; +static bool subaru_angle_main_bus = false; +static bool subaru_angle_invalid = false; static uint32_t subaru_get_checksum(const CANPacket_t *msg) { return (uint8_t)msg->data[0]; @@ -80,8 +104,21 @@ static void subaru_rx_hook(const CANPacket_t *msg) { update_sample(&torque_driver, torque_driver_new); } + if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_Steering_2) && (msg->bus == SUBARU_MAIN_BUS)) { + int angle_meas_new = GET_BYTES_LE(msg, 3, 3) & 0x1FFFFU; + angle_meas_new = -1 * to_signed(angle_meas_new, 17); + update_sample(&angle_meas, angle_meas_new); + } + + if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_ES_Status) && (msg->bus == SUBARU_ALT_BUS)) { + pcm_cruise_check(GET_BIT(msg, 29U)); + } + if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_ES_DashStatus) && (msg->bus == SUBARU_CAM_BUS)) { + acc_main_on = GET_BIT(msg, 49U); + } + // enter controls on rising edge of ACC, exit controls on ACC off - if (msg_matches(msg, MSG_SUBARU_CruiseControl, alt_main_bus)) { + if (!subaru_lkas_angle && msg_matches(msg, MSG_SUBARU_CruiseControl, alt_main_bus)) { bool cruise_engaged = (msg->data[5] >> 1) & 1U; pcm_cruise_check(cruise_engaged); } @@ -110,6 +147,25 @@ 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 = 650 * 100, + .angle_deg_to_can = 100., + .frequency = 50U, + }; + const AngleSteeringLimits SUBARU_FIXED_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}}, + .frequency = 50U, + }; + // Only Crosstrek 2025 selects this VM profile. Match the host CarParams + // vehicle model instead of the frozen generic angle example's Ascent geometry. + const AngleSteeringParams SUBARU_CROSSTREK_2025_ANGLE_PARAMS = { + .slip_factor = -0.0006281955319491566, + .steer_ratio = 17.0, + .wheelbase = 2.6700000762939453, + }; const LongitudinalLimits SUBARU_LONG_LIMITS = { .min_gas = 808, // appears to be engine braking @@ -121,33 +177,54 @@ static bool subaru_tx_hook(const CANPacket_t *msg) { .max_transmission_rpm = 3600, }; - bool tx = true; - bool violation = false; + bool tx = false; + if (!subaru_angle_invalid) { + tx = true; + bool violation = false; - // steer cmd checks - if (msg->addr == MSG_SUBARU_ES_LKAS) { - int desired_torque = ((GET_BYTES_LE(msg, 0, 4) >> 16) & 0x1FFFU); - desired_torque = -1 * to_signed(desired_torque, 13); + // steer cmd checks + if (msg->addr == MSG_SUBARU_ES_LKAS) { + int desired_torque = ((GET_BYTES_LE(msg, 0, 4) >> 16) & 0x1FFFU); + desired_torque = -1 * to_signed(desired_torque, 13); - bool steer_req = (msg->data[3] >> 5) & 1U; + bool steer_req = (msg->data[3] >> 5) & 1U; - const TorqueSteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS : SUBARU_STEERING_LIMITS; - violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits); - } + const TorqueSteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS : SUBARU_STEERING_LIMITS; + violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits); + } + if (msg->addr == MSG_SUBARU_ES_LKAS_ANGLE) { + int desired_angle = GET_BYTES_LE(msg, 5, 3) & 0x1FFFFU; + desired_angle = -1 * to_signed(desired_angle, 17); + bool lkas_request = GET_BIT(msg, 12U); + violation |= lkas_request && !acc_main_on; + if (subaru_angle_main_bus) { + violation |= SAFETY_ABS(desired_angle) > SUBARU_FIXED_ANGLE_STEERING_LIMITS.max_angle; + } + if (subaru_fixed_angle_limits) { + violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_FIXED_ANGLE_STEERING_LIMITS); + } else { + violation |= steer_angle_cmd_checks_vm(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS, SUBARU_CROSSTREK_2025_ANGLE_PARAMS); + } + } - // check es_distance cruise_throttle limits - if (msg->addr == MSG_SUBARU_ES_Distance) { - int cruise_throttle = (GET_BYTES_LE(msg, 2, 2) & 0x1FFFU); - bool cruise_cancel = (msg->data[7] >> 0) & 1U; + if (msg->addr == MSG_SUBARU_ES_LKAS_Alert) { + violation |= subaru_get_checksum(msg) != subaru_compute_checksum(msg); + } - // If openpilot is not controlling long, only allow ES_Distance for cruise cancel requests, - // (when Cruise_Cancel is true, and Cruise_Throttle is inactive) - violation |= (cruise_throttle != SUBARU_LONG_LIMITS.inactive_gas); - violation |= (!cruise_cancel); - } + // check es_distance cruise_throttle limits + if (msg->addr == MSG_SUBARU_ES_Distance) { + int cruise_throttle = (GET_BYTES_LE(msg, 2, 2) & 0x1FFFU); + bool cruise_cancel = (msg->data[7] >> 0) & 1U; - if (violation) { - tx = false; + // If openpilot is not controlling long, only allow ES_Distance for cruise cancel requests, + // (when Cruise_Cancel is true, and Cruise_Throttle is inactive) + violation |= (cruise_throttle != SUBARU_LONG_LIMITS.inactive_gas); + violation |= (!cruise_cancel); + } + + if (violation){ + tx = false; + } } return tx; @@ -163,16 +240,48 @@ static safety_config subaru_init(uint16_t param) { SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS) SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS) }; + static const CanMsg SUBARU_GEN2_ANGLE_TX_MSGS_LIST[] = { + SUBARU_GEN2_ANGLE_TX_MSGS(SUBARU_ALT_BUS) + }; + static const CanMsg SUBARU_ASCENT_ANGLE_TX_MSGS_LIST[] = { + SUBARU_GEN2_ANGLE_TX_MSGS(SUBARU_MAIN_BUS) + }; + + static const CanMsg SUBARU_ASCENT_ALERT_TX_MSGS[] = { + SUBARU_GEN2_ANGLE_TX_MSGS(SUBARU_MAIN_BUS) + {MSG_SUBARU_ES_LKAS_Alert, SUBARU_MAIN_BUS, 8, .check_relay = true}, + }; + + const uint16_t SUBARU_PARAM_ASCENT_ALERT = 2048; + const bool ascent_alert = param == (1U | 16U | 128U | 32U | SUBARU_PARAM_ASCENT_ALERT); const uint16_t SUBARU_PARAM_GEN2 = 1; + const uint16_t SUBARU_PARAM_LKAS_ANGLE = 16; + const uint16_t SUBARU_PARAM_FIXED_ANGLE = 128; + + const uint16_t SUBARU_PARAM_ANGLE_MAIN_BUS = 32; + subaru_angle_main_bus = GET_FLAG(param, SUBARU_PARAM_ANGLE_MAIN_BUS); subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2); + subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE); + subaru_fixed_angle_limits = GET_FLAG(param, SUBARU_PARAM_FIXED_ANGLE); + subaru_angle_invalid = (subaru_lkas_angle && (param != (SUBARU_PARAM_GEN2 | SUBARU_PARAM_LKAS_ANGLE)) && + (param != (SUBARU_PARAM_GEN2 | SUBARU_PARAM_LKAS_ANGLE | SUBARU_PARAM_FIXED_ANGLE)) && + (param != (SUBARU_PARAM_GEN2 | SUBARU_PARAM_LKAS_ANGLE | SUBARU_PARAM_FIXED_ANGLE | SUBARU_PARAM_ANGLE_MAIN_BUS)) && !ascent_alert) || + ((subaru_fixed_angle_limits || subaru_angle_main_bus) && !subaru_lkas_angle); // TODO: re-enable once more work is done on the limits // revert this in the PR that re-enables Subaru longitudinal: https://github.com/commaai/opendbc/pull/3689 safety_config ret; - if (subaru_gen2) { + if (subaru_lkas_angle) { + static RxCheck subaru_gen2_angle_rx_checks[] = { + SUBARU_GEN2_ANGLE_RX_CHECKS() + }; + ret = ascent_alert ? BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_ASCENT_ALERT_TX_MSGS) : + subaru_angle_main_bus ? BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_ASCENT_ANGLE_TX_MSGS_LIST) : + BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_GEN2_ANGLE_TX_MSGS_LIST); + } else if (subaru_gen2) { static RxCheck subaru_gen2_rx_checks[] = { SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS) }; diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru_ascent_alert.py b/opendbc_repo/opendbc/safety/tests/test_subaru_ascent_alert.py new file mode 100644 index 0000000000..c32728ba21 --- /dev/null +++ b/opendbc_repo/opendbc/safety/tests/test_subaru_ascent_alert.py @@ -0,0 +1,41 @@ +import unittest + +from opendbc.car import structs +from opendbc.safety.tests.libsafety import libsafety_py + +DEFAULT_ALERT = bytes.fromhex("ce02000104000000") + + +class TestSubaruAscentAlert(unittest.TestCase): + def setUp(self): + self.safety = libsafety_py.libsafety + + def mode(self, flags): + self.safety.set_safety_hooks(structs.CarParams.SafetyModel.subaru, flags) + self.safety.init_tests() + + def packet(self, data=DEFAULT_ALERT, bus=0): + return libsafety_py.make_CANPacket(0x3C4, bus, data) + + def test_exact_parameter_bus_checksum_length_and_forwarding(self): + for flags in (0, 1, 0x11, 0x91, 0xB1, 0x800, 0x811, 0x891, 0x8B3, 0x9B1): + self.mode(flags) + self.assertFalse(self.safety.safety_tx_hook(self.packet()), hex(flags)) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x3C4), 0) + self.mode(0x8B1) + self.assertTrue(self.safety.safety_tx_hook(self.packet())) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x3C4), -1) + self.assertEqual(self.safety.safety_fwd_hook(0, 0x3C4), 2) + for bus in (1, 2): + self.assertFalse(self.safety.safety_tx_hook(self.packet(bus=bus))) + for data in (bytes.fromhex("0002000104000000"), bytes(7), bytes(12)): + self.assertFalse(self.safety.safety_tx_hook(self.packet(data))) + + def test_stock_main_frame_and_relay_malfunction_revoke(self): + self.mode(0x8B1) + self.safety.safety_rx_hook(self.packet(bus=2)) + self.assertFalse(self.safety.get_relay_malfunction()) + self.safety.safety_rx_hook(self.packet()) + self.assertTrue(self.safety.get_relay_malfunction()) + self.assertFalse(self.safety.safety_tx_hook(self.packet())) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x3C4), -1) diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru_ascent_angle.py b/opendbc_repo/opendbc/safety/tests/test_subaru_ascent_angle.py new file mode 100644 index 0000000000..420d132015 --- /dev/null +++ b/opendbc_repo/opendbc/safety/tests/test_subaru_ascent_angle.py @@ -0,0 +1,162 @@ +import unittest + +from opendbc.can import CANPacker +from opendbc.car import Bus, structs +from opendbc.car.subaru import subarucan +from opendbc.car.subaru.values import CAR, DBC, SubaruSafetyFlags +from opendbc.car.subaru.tests.test_ascent_angle import setup_controller, step +from opendbc.safety.tests.libsafety import libsafety_py +from opendbc.safety.tests import test_subaru_gen2_angle_pair as pair + + +class TestSubaruAscentAngle(unittest.TestCase): + def setUp(self): + self.safety = libsafety_py.libsafety + self.cp, self.controller, self.command, self.state = setup_controller() + self.packer = CANPacker(DBC[CAR.SUBARU_ASCENT_2023][Bus.pt]) + + @staticmethod + def packet(frame): + return libsafety_py.make_CANPacket(frame[0], frame[2], frame[1]) + + def arm(self, **changes): + cfg = self.cp.safetyConfigs[0] + self.assertEqual(self.safety.set_safety_hooks(cfg.safetyModel.raw, cfg.safetyParam), 0) + self.safety.init_tests() + self.safety.set_timer(1_000_000) + speed = changes.pop('speed', 30.3 * 3.6) + _, frames = pair.TestSubaruGen2AnglePair.sources(CAR.SUBARU_ASCENT_2023, speed=speed, **changes) + for frame in frames: + if frame[0] in {0x40, 0x119, 0x11A, 0x13A, 0x13C, 0x222, 0x321}: + self.assertTrue(self.safety.safety_rx_hook(self.packet(frame))) + for counter in range(1, 6): + wheel = self.packer.make_can_msg("Wheel_Speeds", 1, {"FL": speed, "FR": speed, "RL": speed, "RR": speed, "COUNTER": counter}) + self.assertTrue(self.safety.safety_rx_hook(self.packet(wheel))) + self.safety.safety_tick() + self.assertTrue(self.safety.safety_config_valid()) + + def angle(self, value, active=True, bus=0): + return self.packet(subarucan.create_steering_control_angle(self.packer, value, active, bus)) + + def test_host_inactive_entry_and_reentry_match_native_history(self): + self.arm() + self.state.out.vEgoRaw = 30.3 + self.state.out.steeringAngleDeg = 0.78 + self.state.out.steeringRateDeg = -1.5 + self.controller.ascent_handoff_active = True + self.safety.set_angle_meas(78, 78) + steer, decoded = step(self.controller, self.command, self.state) + self.assertFalse(decoded['LKAS_Request']) + self.assertTrue(self.safety.safety_tx_hook(self.packet(steer))) + self.state.out.steeringAngleDeg = 0.74 + self.state.out.steeringRateDeg = -1.99 + self.safety.set_angle_meas(74, 74) + steer, decoded = step(self.controller, self.command, self.state) + self.assertAlmostEqual(decoded['LKAS_Output'], 0.53, delta=0.01) + self.assertTrue(self.safety.safety_tx_hook(self.packet(steer))) + self.assertFalse(self.safety.safety_tx_hook(self.angle(-1.45))) + + def test_first_nonzero_angle_primes_native_history_before_request(self): + self.arm() + self.state.out.vEgoRaw = 30.3 + self.state.out.steeringAngleDeg = 20.0 + self.state.out.steeringRateDeg = 0.0 + self.safety.set_angle_meas(2000, 2000) + steer, decoded = step(self.controller, self.command, self.state) + self.assertFalse(decoded['LKAS_Request']) + self.assertTrue(self.safety.safety_tx_hook(self.packet(steer))) + steer, decoded = step(self.controller, self.command, self.state) + self.assertTrue(decoded['LKAS_Request']) + self.assertTrue(self.safety.safety_tx_hook(self.packet(steer))) + + def test_buses_angle_bounds_permission_and_rate_limits(self): + for bad in (self.angle(0, bus=1), self.angle(0, bus=2), self.angle(546)): + self.arm() + self.assertFalse(self.safety.safety_tx_hook(bad)) + self.arm() + self.safety.set_desired_angle_last(0) + self.assertTrue(self.safety.safety_tx_hook(self.angle(0.25))) + self.assertFalse(self.safety.safety_tx_hook(self.angle(1.0))) + for changes in ({'main': False}, {'cruise': False}, {'brake': True}): + self.arm(**changes) + self.assertFalse(self.safety.safety_tx_hook(self.angle(0))) + self.assertTrue(self.safety.safety_tx_hook(self.angle(0, active=False))) + for flag in (SubaruSafetyFlags.LONG, 0x100, 0x40): + cfg = self.cp.safetyConfigs[0] + self.safety.set_safety_hooks(cfg.safetyModel.raw, cfg.safetyParam | flag) + self.safety.init_tests() + self.safety.set_controls_allowed(True) + self.assertFalse(self.safety.safety_tx_hook(self.angle(0))) + + def test_forwarding_and_exact_parameter_discriminator(self): + self.arm() + self.assertEqual(self.safety.safety_fwd_hook(2, 0x124), -1) + self.assertEqual(self.safety.safety_fwd_hook(0, 0x124), 2) + self.assertEqual(self.safety.safety_fwd_hook(1, 0x124), -1) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x221), 0) + self.assertFalse(self.safety.safety_tx_hook(self.packet(self.packer.make_can_msg('ES_LKAS', 0, {})))) + self.assertFalse(self.safety.safety_tx_hook(self.packet(self.packer.make_can_msg('ES_Brake', 1, {})))) + self.safety.set_desired_angle_last(54500) + self.assertFalse(self.safety.safety_tx_hook(self.angle(546))) + self.arm() + self.safety.set_desired_angle_last(-54500) + self.assertFalse(self.safety.safety_tx_hook(self.angle(-546))) + for invalid in (0x20, 0x21, 0x30, 0x31, 0xA0, 0xB0, 0xB3, 0x1B1): + self.safety.set_safety_hooks(structs.CarParams.SafetyModel.subaru, invalid) + self.safety.init_tests() + self.safety.set_controls_allowed(True) + for bus in (0, 1, 2): + self.assertFalse(self.safety.safety_tx_hook(self.angle(0, bus=bus)), hex(invalid)) + self.arm() + self.safety.set_relay_malfunction(True) + self.assertEqual(self.safety.safety_fwd_hook(0, 0x124), -1) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x221), -1) + + def test_required_can_loss_and_relay_revoke(self): + self.arm() + self.assertTrue(self.safety.safety_tx_hook(self.angle(0))) + self.safety.set_timer(10_000_000) + self.safety.safety_tick() + self.assertFalse(self.safety.safety_config_valid()) + self.assertFalse(self.safety.safety_tx_hook(self.angle(0))) + self.arm() + self.safety.set_relay_malfunction(True) + self.assertFalse(self.safety.safety_tx_hook(self.angle(0))) + + def test_received_stock_angle_detects_relay(self): + for bus in (1, 2): + self.arm() + self.safety.safety_rx_hook(self.angle(0, active=False, bus=bus)) + self.assertFalse(self.safety.get_relay_malfunction()) + self.arm() + self.safety.safety_rx_hook(self.angle(0, active=False)) + self.assertTrue(self.safety.get_relay_malfunction()) + self.assertFalse(self.safety.safety_tx_hook(self.angle(0))) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x221), -1) + + def test_inactive_measured_beyond_fixed_bound_is_safely_clamped(self): + for measured in (-600.0, 600.0): + with self.subTest(measured=measured): + self.arm() + _, self.controller, self.command, self.state = setup_controller() + self.command.latActive = False + self.state.out.steeringAngleDeg = measured + self.safety.set_angle_meas(int(measured * 100), int(measured * 100)) + steer, decoded = step(self.controller, self.command, self.state) + self.assertEqual(abs(decoded['LKAS_Output']), 545.0) + self.assertTrue(self.safety.safety_tx_hook(self.packet(steer))) + + def test_driver_override_handoff_frames_match_native(self): + self.arm() + self.state.out.vEgoRaw = 30.3 + for torque, rate, measured, active in ((0, 0, 0, False), (0, 0, 0, True), + (250, 35, 0, True), (250, 35, 0, False), + (150, 0, 0, False), (149, 3, 0, False), + (149, 0, 0, False), (149, 0, 0, True), (0, 0, 0, True)): + self.state.out.steeringTorque = torque + self.state.out.steeringRateDeg = rate + self.state.out.steeringAngleDeg = measured + self.safety.set_angle_meas(int(measured * 100), int(measured * 100)) + steer, decoded = step(self.controller, self.command, self.state) + self.assertEqual(bool(decoded['LKAS_Request']), active, (torque, rate)) + self.assertTrue(self.safety.safety_tx_hook(self.packet(steer))) diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru_gen2_angle_pair.py b/opendbc_repo/opendbc/safety/tests/test_subaru_gen2_angle_pair.py new file mode 100644 index 0000000000..8710e6bdb7 --- /dev/null +++ b/opendbc_repo/opendbc/safety/tests/test_subaru_gen2_angle_pair.py @@ -0,0 +1,289 @@ +import unittest + +from opendbc.can import CANPacker +from opendbc.car import Bus, structs +from opendbc.car.fw_versions import match_fw_to_car +from opendbc.car.subaru.carcontroller import CarController +from opendbc.car.subaru.carstate import CarState +from opendbc.car.subaru.interface import CarInterface +from opendbc.car.subaru.values import CAR, DBC, SubaruSafetyFlags +from opendbc.safety.tests.libsafety import libsafety_py + + +CARS = (CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025) + + +class TestSubaruGen2AnglePair(unittest.TestCase): + def setUp(self): + self.safety = libsafety_py.libsafety + + @staticmethod + def packet(frame): + return libsafety_py.make_CANPacket(frame[0], frame[2], frame[1]) + + @staticmethod + def params(car, *, alpha=False, release=False): + return CarInterface.get_params(car, {0: {}, 1: {}, 2: {}}, [], alpha, release, False) + + def mode(self, cp, *, extra_flags=0): + cfg = cp.safetyConfigs[0] + self.assertEqual(self.safety.set_safety_hooks(cfg.safetyModel.raw, cfg.safetyParam | extra_flags), 0) + self.safety.init_tests() + self.safety.set_timer(1_000_000) + + @staticmethod + def sources(car, *, brake=False, cruise=True, speed=30, main=True, torque=0): + packer = CANPacker(DBC[car][Bus.pt]) + inputs = ( + ('Throttle', 0, {}), + ('Steering_Torque', 0, {'Steer_Torque_Sensor': torque}), + ('Steering_2', 0, {'Steering_Angle': 0}), + ('Wheel_Speeds', 1, {'FL': speed, 'FR': speed, 'RL': speed, 'RR': speed}), + ('Brake_Status', 1, {'Brake': int(brake)}), + ('ES_Status', 1, {'Cruise_Activated': int(cruise)}), + ('ES_DashStatus', 2, {'Cruise_On': int(main)}), + ('Transmission', 0, {'Gear': 121}), + ('Dashlights', 0, {}), + ('BodyInfo', 0, {}), + ('ES_Distance', 1, {}), + ('ES_LKAS_State', 2, {}), + ('ES_Brake', 1, {}), + ) + return packer, [packer.make_can_msg(name, bus, values) for name, bus, values in inputs] + + def controller(self, car, cp, frames, *, active=True): + state = CarState(cp) + parsers = state.get_can_parsers(cp) + state.update(parsers) + for parser in parsers.values(): + parser.update([(1_000_000_000, frames)]) + state.out = state.update(parsers).as_reader() + self.assertEqual(state.out.gearShifter, structs.CarState.GearShifter.drive) + self.assertGreater(state.out.vEgoRaw, 0) + + command = structs.CarControl() + command.enabled = True + command.latActive = active + command.actuators.steeringAngleDeg = 1.0 + _, sends = CarController(DBC[car], cp).update(command.as_reader(), state, 1_100_000_000) + steer = next(frame for frame in sends if frame[0] == 0x124) + self.assertEqual(steer[2], 1) + self.assertEqual(bool(steer[1][1] & 0x10), active and state.out.cruiseState.available and + state.out.cruiseState.enabled and not state.out.brakePressed) + return state, steer, sends + + def arm(self, cp, frames): + self.mode(cp) + required = {0x40, 0x119, 0x11A, 0x13A, 0x13C, 0x222, 0x321} + for frame in frames: + if frame[0] in required: + self.assertTrue(self.safety.safety_rx_hook(self.packet(frame)), hex(frame[0])) + self.safety.safety_tick() + self.assertTrue(self.safety.safety_config_valid()) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_stock_packed_host_to_native_and_inactive(self): + for car in CARS: + for release in (False, True): + with self.subTest(car=car, release=release): + cp = self.params(car, alpha=True, release=release) + self.assertFalse(cp.dashcamOnly) + self.assertFalse(cp.openpilotLongitudinalControl) + self.assertFalse(cp.alphaLongitudinalAvailable) + self.assertEqual(cp.steerControlType, structs.CarParams.SteerControlType.angle) + packer, frames = self.sources(car) + state, steer, sends = self.controller(car, cp, frames) + self.assertTrue(state.out.cruiseState.enabled) + self.assertNotEqual(steer[1][5:7], b'\x00\x00') + self.arm(cp, frames) + self.assertTrue(self.safety.safety_tx_hook(self.packet(steer))) + self.assertEqual({frame[0] for frame in sends}, {0x124, 0x321, 0x322}) + self.assertFalse(self.safety.safety_tx_hook(self.packet((0x122, bytes(8), 0)))) + self.assertFalse(self.safety.safety_tx_hook(self.packet((0x124, steer[1], 0)))) + self.assertFalse(self.safety.safety_tx_hook(self.packet((0x124, steer[1][:-1], 1)))) + self.assertFalse(self.safety.safety_tx_hook(self.packet(packer.make_can_msg('ES_Distance', 1, {})))) + cancel = structs.CarControl() + cancel.cruiseControl.cancel = True + _, cancel_sends = CarController(DBC[car], cp).update(cancel.as_reader(), state, 1_100_000_000) + distance = next(frame for frame in cancel_sends if frame[0] == 0x221) + self.assertEqual(distance[2], 1) + self.assertTrue(self.safety.safety_tx_hook(self.packet(distance))) + + _, inactive, _ = self.controller(car, cp, frames, active=False) + self.assertTrue(self.safety.safety_tx_hook(self.packet(inactive))) + brake_frames = [packer.make_can_msg('Brake_Status', 1, {'Brake': 1}) if f[0] == 0x13C else f for f in frames] + brake_state, brake_steer, _ = self.controller(car, cp, brake_frames) + self.assertTrue(brake_state.out.brakePressed) + self.assertFalse(brake_steer[1][1] & 0x10) + self.assertTrue(self.safety.safety_rx_hook(self.packet(brake_frames[4]))) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertTrue(self.safety.safety_tx_hook(self.packet(brake_steer))) + self.assertFalse(self.safety.safety_tx_hook(self.packet(steer))) + + def test_required_health_and_raw_long_conflict(self): + for car in CARS: + cp = self.params(car) + _, frames = self.sources(car) + required = [f for f in frames if f[0] in {0x40, 0x119, 0x11A, 0x13A, 0x13C, 0x222, 0x321}] + for missing in required: + with self.subTest(car=car, missing=hex(missing[0])): + self.mode(cp) + for frame in required: + if frame is not missing: + self.safety.safety_rx_hook(self.packet(frame)) + self.safety.safety_tick() + self.assertFalse(self.safety.safety_config_valid()) + self.arm(cp, frames) + _, steer, _ = self.controller(car, cp, frames) + for flags in (SubaruSafetyFlags.LONG, 0x20, 0x100): + with self.subTest(car=car, flags=flags): + self.mode(cp, extra_flags=flags) + for frame in required: + self.safety.safety_rx_hook(self.packet(frame)) + self.safety.safety_tick() + self.assertFalse(self.safety.safety_tx_hook(self.packet(steer))) + for raw_param in (0x10, 0x30, 0x31, 0x111): + with self.subTest(car=car, raw_param=hex(raw_param)): + self.assertEqual(self.safety.set_safety_hooks(structs.CarParams.SafetyModel.subaru, raw_param), 0) + self.safety.init_tests() + for frame in required: + self.safety.safety_rx_hook(self.packet(frame)) + self.safety.safety_tick() + self.assertFalse(self.safety.safety_tx_hook(self.packet(steer))) + self.mode(cp) + for frame in required: + if frame[0] == 0x222: + frame = (frame[0], frame[1][:-1] + bytes([frame[1][-1] ^ 1]), frame[2]) + self.safety.safety_rx_hook(self.packet(frame)) + self.safety.safety_tick() + self.assertFalse(self.safety.safety_config_valid()) + + for source in required: + for bad in ((source[0], source[1], (source[2] + 1) % 3), + (source[0], source[1][:-1], source[2])): + with self.subTest(car=car, source=hex(source[0]), bad=bad[2:]): + self.mode(cp) + for frame in required: + self.safety.safety_rx_hook(self.packet(bad if frame is source else frame)) + self.safety.safety_tick() + self.assertFalse(self.safety.safety_config_valid()) + + self.arm(cp, frames) + self.safety.set_timer(10_000_000) + self.safety.safety_tick() + self.assertFalse(self.safety.safety_config_valid()) + + self.mode(cp) + for frame in required: + self.safety.safety_rx_hook(self.packet(frame)) + for _ in range(6): + self.safety.safety_rx_hook(self.packet(next(f for f in required if f[0] == 0x222))) + self.safety.safety_tick() + self.assertFalse(self.safety.safety_config_valid()) + + def test_stock_main_driver_and_cruise_handoff(self): + for car in CARS: + cp = self.params(car) + for override in ({'main': False}, {'cruise': False}, {'brake': True}, {'torque': 250}): + with self.subTest(car=car, override=override): + _, frames = self.sources(car, **override) + self.mode(cp) + for frame in frames: + self.safety.safety_rx_hook(self.packet(frame)) + state, steer, _ = self.controller(car, cp, frames) + if override.get('torque'): + # The host deliberately waits for a second steering period before + # cutting a request on persistent driver input. + command = structs.CarControl() + command.enabled = command.latActive = True + command.actuators.steeringAngleDeg = 1.0 + controller = CarController(DBC[car], cp) + controller.update(command.as_reader(), state, 1_100_000_000) + controller.update(command.as_reader(), state, 1_110_000_000) + _, sends = controller.update(command.as_reader(), state, 1_120_000_000) + steer = next(frame for frame in sends if frame[0] == 0x124) + self.assertFalse(steer[1][1] & 0x10) + self.assertTrue(self.safety.safety_tx_hook(self.packet(steer))) + + _, frames = self.sources(car) + self.arm(cp, frames) + _, steer, _ = self.controller(car, cp, frames) + self.assertTrue(self.safety.safety_tx_hook(self.packet(steer))) + _, main_off = self.sources(car, main=False) + dash = next(frame for frame in main_off if frame[0] == 0x321) + self.assertTrue(self.safety.safety_rx_hook(self.packet(dash))) + self.assertFalse(self.safety.safety_tx_hook(self.packet(steer))) + + def test_forwarding_and_relay(self): + for car in CARS: + cp = self.params(car) + self.mode(cp) + self.assertEqual(self.safety.safety_fwd_hook(0, 0x124), 2) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x124), 0) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x321), -1) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x322), -1) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x221), 0) + self.assertEqual(self.safety.safety_fwd_hook(1, 0x124), -1) + _, frames = self.sources(car) + _, steer, _ = self.controller(car, cp, frames) + self.arm(cp, frames) + self.assertTrue(self.safety.safety_tx_hook(self.packet(steer))) + self.safety.set_relay_malfunction(True) + self.assertFalse(self.safety.safety_tx_hook(self.packet(steer))) + self.assertEqual(self.safety.safety_fwd_hook(0, 0x124), -1) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x221), -1) + + def test_host_native_angle_rate_matrix(self): + for car in CARS: + for speed in (5, 30, 80): + for target in (-650., 650.): + with self.subTest(car=car, speed=speed, target=target): + cp = self.params(car) + _, frames = self.sources(car, speed=speed) + state = CarState(cp) + parsers = state.get_can_parsers(cp) + state.update(parsers) + for parser in parsers.values(): + parser.update([(1_000_000_000, frames)]) + state.out = state.update(parsers).as_reader() + self.arm(cp, frames) + controller = CarController(DBC[car], cp) + command = structs.CarControl() + command.enabled = command.latActive = True + command.actuators.steeringAngleDeg = target + seen = [] + for i in range(120): + self.safety.set_timer(1_000_000 + i * 10_000) + _, sends = controller.update(command.as_reader(), state, 1_000_000_000 + i * 10_000_000) + for frame in sends: + if frame[0] == 0x124: + self.assertTrue(self.safety.safety_tx_hook(self.packet(frame)), + f'{car.value} {speed=} {target=} {i=} {frame[1].hex()}') + seen.append(frame) + self.assertGreater(len(seen), 50) + self.assertNotEqual(seen[-1][1][5:7], b'\x00\x00') + + def test_firmware_complete_partial_and_mixed(self): + # The two identities stay manual-only: the production exact matcher treats + # multiple observed versions at one ECU address as alternatives. A mixed + # ABS observation can otherwise select a sibling with false confidence. + e = structs.CarParams.Ecu + versions = { + CARS[0]: [(e.abs, 0x7b0, b'\xa2 $\x15\x05'), (e.fwdCamera, 0x787, b'\x1d!\x08\x00F\x14!\x08\x00='), + (e.engine, 0x7a2, b'\x04"cP\x07')], + CARS[1]: [(e.abs, 0x7b0, b'\xa1 $\x11\x00'), (e.eps, 0x746, b'[\xc0\xd1\x10\x00'), + (e.fwdCamera, 0x787, b'\x1a!\x08\x00C\x0e!\x08\x018'), + (e.engine, 0x7a2, b'\x08,\xa0p\x07'), (e.transmission, 0x7a3, b'\xeb\x17U!r')], + } + + def records(entries): + return [structs.CarParams.CarFw(ecu=ecu, address=addr, fwVersion=version, brand='subaru') + for ecu, addr, version in entries] + + for car in CARS: + for entries in (versions[car], versions[car][:1], versions[car][1:2]): + _, matches = match_fw_to_car(records(entries), '00000000000000000', log=False) + self.assertNotIn(car, matches) + mixed = versions[CARS[0]][:1] + versions[CARS[1]] + _, matches = match_fw_to_car(records(mixed), '00000000000000000', log=False) + self.assertFalse(set(CARS) & matches)