From bd9d65e92c9c01a768d6f3bd2bfe38332595c3fa Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 11 Aug 2026 21:13:15 -0500 Subject: [PATCH] Bionic Commando --- opendbc_repo/opendbc/car/gm/carcontroller.py | 2 - .../car/gm/tests/test_carcontroller.py | 2 +- .../opendbc/car/subaru/carcontroller.py | 31 +++++++++-- opendbc_repo/opendbc/car/subaru/interface.py | 2 + .../opendbc/car/subaru/tests/test_subaru.py | 34 +++++++++++- opendbc_repo/opendbc/car/subaru/values.py | 6 +++ opendbc_repo/opendbc/safety/modes/subaru.h | 24 ++++++++- .../opendbc/safety/tests/test_subaru.py | 10 ++++ .../test/unit/test_usb_mmio_scalar.py | 54 +++++++++++++++++++ tinygrad_repo/tinygrad/runtime/support/usb.py | 20 ++++++- 10 files changed, 176 insertions(+), 9 deletions(-) create mode 100644 tinygrad_repo/test/unit/test_usb_mmio_scalar.py diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index a6d64795b..b8152bd92 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -204,8 +204,6 @@ def should_send_acc_2cd(CP): return ( CP.networkLocation == NetworkLocation.fwdCamera and CP.carFingerprint in CAMERA_ACC_CAR and - # The Trailblazer already supplies this counter on its camera bus. - CP.carFingerprint != CAR.CHEVROLET_TRAILBLAZER and CP.carFingerprint not in (CC_ONLY_CAR | SDGM_CAR) and not bool(getattr(CP, "flags", 0) & GMFlags.NO_CAMERA.value) ) diff --git a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py index 562da811f..2761fdc79 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py @@ -361,7 +361,7 @@ def test_live_camera_path_does_not_send_pt_keepalive(): def test_acc_2cd_replacement_only_used_with_live_camera_path(): - assert not should_send_acc_2cd(SimpleNamespace( + assert should_send_acc_2cd(SimpleNamespace( carFingerprint=CAR.CHEVROLET_TRAILBLAZER, networkLocation=CarParams.NetworkLocation.fwdCamera, flags=0)) assert should_send_acc_2cd(SimpleNamespace( carFingerprint=CAR.CHEVROLET_SILVERADO, networkLocation=CarParams.NetworkLocation.fwdCamera, flags=0)) diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index c9db33497..d777f714b 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -1,10 +1,10 @@ import numpy as np from opendbc.can import CANPacker -from opendbc.car import Bus, DT_CTRL, make_tester_present_msg -from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance +from opendbc.car import Bus, DT_CTRL, 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 @@ -14,6 +14,8 @@ MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut _SNG_ACC_MIN_DIST = 3 _SNG_ACC_MAX_DIST = 4.5 +_LEGACY_2025_MADS_MIN_SPEED = 0.44704 +_LEGACY_2025_MADS_MAX_STEER_ANGLE = 120.0 def get_safety_CP(): @@ -27,6 +29,7 @@ class CarController(CarControllerBase): self.apply_torque_last = 0 self.apply_steer_last = 0 self.driver_override = False + self.legacy_2025_lkas_active = False self.cruise_button_prev = 0 self.steer_rate_counter = 0 @@ -45,6 +48,28 @@ class CarController(CarControllerBase): self.last_standstill_frame = 0 def lateral_angle(self, CC, CS): + if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025: + mads_only = CC.latActive and not CC.enabled + mads_only_ok = CS.out.vEgoRaw > _LEGACY_2025_MADS_MIN_SPEED and \ + abs(CS.out.steeringAngleDeg) < _LEGACY_2025_MADS_MAX_STEER_ANGLE + lkas_active = CC.latActive and (not mads_only or mads_only_ok) and \ + CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill + + if lkas_active and not self.legacy_2025_lkas_active: + self.apply_steer_last = CS.out.steeringAngleDeg + + apply_steer = apply_std_steer_angle_limits( + CC.actuators.steeringAngleDeg, + self.apply_steer_last, + CS.out.vEgoRaw, + CS.out.steeringAngleDeg, + lkas_active, + self.p.LEGACY_2025_ANGLE_LIMITS, + ) + self.apply_steer_last = apply_steer + self.legacy_2025_lkas_active = lkas_active + return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus) + abs_torque = abs(CS.out.steeringTorque) if abs_torque > self.p.STEER_OVERRIDE_TORQUE_HIGH: self.driver_override = True diff --git a/opendbc_repo/opendbc/car/subaru/interface.py b/opendbc_repo/opendbc/car/subaru/interface.py index f1d7c04e2..9432bcb8c 100644 --- a/opendbc_repo/opendbc/car/subaru/interface.py +++ b/opendbc_repo/opendbc/car/subaru/interface.py @@ -40,6 +40,8 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM.value if ret.flags & SubaruFlags.D_PLATFORM_CAMERA: ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value + if candidate == CAR.SUBARU_LEGACY_2025: + ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS.value ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index 7f13f583e..9bb2276d2 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -5,7 +5,7 @@ from types import SimpleNamespace import pytest from opendbc.can import CANPacker, CANParser -from opendbc.car import Bus +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 @@ -165,6 +165,7 @@ def test_outback_2023_uses_d_platform_bus_layout(): assert CP.flags & SubaruFlags.D_PLATFORM assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM + assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS) assert CanBus.main_for_cp(CP) == CanBus.alt assert CanBus.angle_for_cp(CP) == CanBus.main assert parsers[Bus.pt].bus == CanBus.alt @@ -184,6 +185,7 @@ def test_legacy_2025_uses_gen2_angle_bus_layout(): assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM) assert not (CP.flags & SubaruFlags.D_PLATFORM_CAMERA) assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM_CAMERA) + assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS assert CanBus.main_for_cp(CP) == CanBus.main assert CanBus.angle_for_cp(CP) == CanBus.main assert parsers[Bus.pt].bus == CanBus.main @@ -194,6 +196,36 @@ def test_legacy_2025_uses_gen2_angle_bus_layout(): assert controller.status_bus == CanBus.main +def test_legacy_2025_uses_validated_angle_request_limits(): + CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025) + controller = CarController({}, CP) + CC = SimpleNamespace( + enabled=False, + latActive=True, + actuators=SimpleNamespace(steeringAngleDeg=-73.05), + ) + CS = SimpleNamespace(out=SimpleNamespace( + vEgoRaw=2.4, + steeringAngleDeg=-75.27, + gearShifter=structs.CarState.GearShifter.drive, + standstill=False, + )) + + msg = controller.lateral_angle(CC, CS) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main) + parser.update([(1, [msg])]) + + assert parser.can_valid + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1 + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(-73.05) + + CS.out.standstill = True + msg = controller.lateral_angle(CC, CS) + parser.update([(2, [msg])]) + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0 + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg) + + def test_ascent_2023_uses_d_platform_bus_layout(): CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023) parsers = CarState.get_can_parsers(CP) diff --git a/opendbc_repo/opendbc/car/subaru/values.py b/opendbc_repo/opendbc/car/subaru/values.py index 15116d0d6..cb19a5869 100644 --- a/opendbc_repo/opendbc/car/subaru/values.py +++ b/opendbc_repo/opendbc/car/subaru/values.py @@ -19,6 +19,11 @@ class CarControllerParams: MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * 0.06), MAX_ANGLE_RATE=1, ) + LEGACY_2025_ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits( + 545, + ([0., 5., 35.], [5., .8, .15]), + ([0., 5., 35.], [5., .8, .15]), + ) def __init__(self, CP): self.STEER_STEP = 2 # how often we update the steer cmd @@ -76,6 +81,7 @@ class SubaruSafetyFlags(IntFlag): LKAS_ANGLE = 16 D_PLATFORM = 32 D_PLATFORM_CAMERA = 64 + LEGACY_2025_ANGLE_LIMITS = 128 class SubaruFlags(IntFlag): diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index dd1f4c26d..c8e735ac2 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -107,6 +107,7 @@ static bool subaru_longitudinal = false; static bool subaru_stop_and_go = false; static bool subaru_lkas_angle = false; static bool subaru_d_platform = false; +static bool subaru_legacy_2025_angle_limits = false; static uint32_t subaru_get_checksum(const CANPacket_t *msg) { return (uint8_t)msg->data[0]; @@ -191,6 +192,20 @@ static bool subaru_tx_hook(const CANPacket_t *msg) { .frequency = 50U, }; + const AngleSteeringLimits SUBARU_LEGACY_2025_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, + }; + const AngleSteeringParams SUBARU_ANGLE_STEERING_PARAMS = { .slip_factor = -0.000580374471400815, .steer_ratio = 13.5, @@ -226,7 +241,11 @@ static bool subaru_tx_hook(const CANPacket_t *msg) { 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); + if (subaru_legacy_2025_angle_limits) { + violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_LEGACY_2025_ANGLE_STEERING_LIMITS); + } else { + violation |= steer_angle_cmd_checks_vm(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS, SUBARU_ANGLE_STEERING_PARAMS); + } } // check es_brake brake_pressure limits @@ -356,6 +375,9 @@ static safety_config subaru_init(uint16_t param) { const uint16_t SUBARU_PARAM_D_PLATFORM_CAMERA = 64; const bool subaru_d_platform_camera = GET_FLAG(param, SUBARU_PARAM_D_PLATFORM_CAMERA); + const uint16_t SUBARU_PARAM_LEGACY_2025_ANGLE_LIMITS = 128; + subaru_legacy_2025_angle_limits = GET_FLAG(param, SUBARU_PARAM_LEGACY_2025_ANGLE_LIMITS); + #ifdef ALLOW_DEBUG const uint16_t SUBARU_PARAM_LONGITUDINAL = 2; subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL); diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru.py b/opendbc_repo/opendbc/safety/tests/test_subaru.py index cf9b6695a..787baf851 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru.py @@ -208,6 +208,8 @@ class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafety super().setUp() def _get_steer_cmd_angle_max(self, speed): + if self.FLAGS & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS: + return self.STEER_ANGLE_MAX return get_max_angle_vm(max(speed, 1), self.VM, CarControllerParams) def _angle_cmd_msg(self, angle, enabled, increment_timer=True): @@ -356,6 +358,14 @@ class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafe TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS, SubaruMsg.ES_LKAS_ANGLE) +class TestSubaruGen2Legacy2025AngleSafety(TestSubaruGen2AngleStockLongitudinalSafety): + FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS + STEER_ANGLE_MAX = 545 + ANGLE_RATE_BP = [0., 5., 35.] + ANGLE_RATE_UP = [5., .8, .15] + ANGLE_RATE_DOWN = [5., .8, .15] + + class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase): FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM ALT_MAIN_BUS = SUBARU_ALT_BUS diff --git a/tinygrad_repo/test/unit/test_usb_mmio_scalar.py b/tinygrad_repo/test/unit/test_usb_mmio_scalar.py new file mode 100644 index 000000000..be216db31 --- /dev/null +++ b/tinygrad_repo/test/unit/test_usb_mmio_scalar.py @@ -0,0 +1,54 @@ +from tinygrad.runtime.support.usb import USBMMIOInterface + + +class FakeUSB: + def __init__(self): + self.calls = [] + + def pcie_mem_req(self, address, value=None, size=4): + self.calls.append(("scalar", address, value, size)) + return 0x11223344 if value is None else None + + def pcie_mem_read(self, address, size): + self.calls.append(("read", address, size)) + return bytes(size) + + def pcie_mem_write(self, address, data): + self.calls.append(("write", address, data)) + + +def test_scalar_mmio_uses_single_tlp(): + usb = FakeUSB() + mmio = USBMMIOInterface(usb, 0x1000, 0x100, "I") + + mmio[2] = 0xAABBCCDD + assert mmio[3] == 0x11223344 + + assert usb.calls == [ + ("scalar", 0x1008, 0xAABBCCDD, 4), + ("scalar", 0x100C, None, 4), + ] + + +def test_slice_mmio_keeps_streaming_path(): + usb = FakeUSB() + mmio = USBMMIOInterface(usb, 0x2000, 0x100, "I") + + mmio[0:2] = b"\x01\x02\x03\x04\x05\x06\x07\x08" + assert mmio[0:2] == bytes(8) + + assert usb.calls == [ + ("write", 0x2000, b"\x01\x02\x03\x04\x05\x06\x07\x08"), + ("read", 0x2000, 8), + ] + + +def test_scalar_mmio_falls_back_to_streaming_transport(): + class StreamingUSB: + def __init__(self): self.data = bytearray(4) + def pcie_mem_read(self, address, size): return self.data[address-0x3000:address-0x3000+size] + def pcie_mem_write(self, address, data): self.data[address-0x3000:address-0x3000+len(data)] = data + + mmio = USBMMIOInterface(StreamingUSB(), 0x3000, 4, "I") + mmio[0] = 0xAABBCCDD + assert mmio[0] == 0xAABBCCDD diff --git a/tinygrad_repo/tinygrad/runtime/support/usb.py b/tinygrad_repo/tinygrad/runtime/support/usb.py index b7e8b8186..a69d5b01a 100644 --- a/tinygrad_repo/tinygrad/runtime/support/usb.py +++ b/tinygrad_repo/tinygrad/runtime/support/usb.py @@ -135,6 +135,9 @@ class CustomASM24Controller: address = (bus << 24) | (dev << 19) | (fn << 16) | (byte_addr & 0xfff) return self.pcie_request(fmt_type, address, value, size) + def pcie_mem_req(self, address:int, value:int|None=None, size:int=4): + return self.pcie_request(0x60 if value is not None else 0x20, address, value, size) + def pcie_mem_write(self, address:int, data:bytes): """Streaming PCIe memory write via 0xF0 mode 1 + bulk OUT. Data is little-endian dwords on the wire.""" if not data: return @@ -183,16 +186,31 @@ class USBMMIOInterface(MMIOInterface): if isinstance(index, slice): return ((index.start or 0) * self.el_sz, ((index.stop or len(self))-(index.start or 0)) * self.el_sz) return (index * self.el_sz, self.el_sz) + def _scalar(self, off:int, size:int, value:int|None=None): + assert size in (1, 2, 4, 8), f"invalid scalar PCIe access size {size}" + if not hasattr(self.usb, "pcie_mem_req"): + if value is not None: + self.usb.pcie_mem_write(self.addr + off, value.to_bytes(size, "little")) + return + return int.from_bytes(self.usb.pcie_mem_read(self.addr + off, size), "little") + upper = 0 if size < 8 else self.usb.pcie_mem_req(self.addr + off + 4, value if value is None else value >> 32, 4) + lower = self.usb.pcie_mem_req(self.addr + off, value if value is None else value & 0xffffffff, min(size, 4)) + if value is None: return lower | (upper << 32) + def __getitem__(self, index): off, sz = self._off_from_index(index) if self.pcimem: + if not isinstance(index, slice): return self._scalar(off, sz) assert sz % 4 == 0 and off % 4 == 0, f"pcie_mem_read requires 4-byte aligned access, got off={off}, sz={sz}" data = self.usb.pcie_mem_read(self.addr + off, sz) else: data = self.usb.scsi_read(sz) if self.addr == 0xf000 else self.usb.read(self.addr + off, sz) return int.from_bytes(data, "little") if sz == self.el_sz else data def __setitem__(self, index, data): - off, _ = self._off_from_index(index) + off, sz = self._off_from_index(index) + if self.pcimem and not isinstance(index, slice) and isinstance(data, int): + self._scalar(off, sz, data) + return data = struct.pack(self.fmt, data) if isinstance(data, int) else bytes(data) if not self.pcimem: self.usb.scsi_write(data) if self.addr == 0xf000 else self.usb.write(self.addr + off, data) else: self.usb.pcie_mem_write(self.addr+off, data)