From 34ae63cbf6bdeb2ecc99481698ce3dc1329177c4 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 29 Oct 2026 12:06:38 -0500 Subject: [PATCH] Adapt jacobw Subaru angle control to StarPilot --- CREDITS.md | 2 + opendbc_repo/opendbc/car/subaru/JACOBW.md | 23 +++ .../opendbc/car/subaru/carcontroller.py | 6 + .../opendbc/car/subaru/jacobw_angle.py | 31 ++++ .../car/subaru/tests/test_angle_handoff.py | 10 +- .../car/subaru/tests/test_jacobw_angle.py | 133 ++++++++++++++++++ .../car/subaru/tests/test_output_contract.py | 4 +- opendbc_repo/opendbc/safety/modes/subaru.h | 6 +- .../opendbc/safety/modes/subaru_jacobw.h | 6 + .../tests/test_subaru_gen2_angle_pair.py | 4 +- prebuilt.json | 2 +- 11 files changed, 219 insertions(+), 8 deletions(-) create mode 100644 opendbc_repo/opendbc/car/subaru/JACOBW.md create mode 100644 opendbc_repo/opendbc/car/subaru/jacobw_angle.py create mode 100644 opendbc_repo/opendbc/car/subaru/tests/test_jacobw_angle.py create mode 100644 opendbc_repo/opendbc/safety/modes/subaru_jacobw.h diff --git a/CREDITS.md b/CREDITS.md index febe479667..e909915393 100644 --- a/CREDITS.md +++ b/CREDITS.md @@ -43,3 +43,5 @@ The default-off Use Pinion Yaw Sensor measurement option is adapted from jrneuli Subaru angle support includes implementation maintained and integrated by **Jacob Waller (jacobw)** in [jacobwaller/openpilot, jul-angle-based](https://github.com/jacobwaller/openpilot/tree/542816196a18dba5f6f0af093c1ece4f415b5a22), with its [opendbc implementation at 8acbeccdab45a47aa7af170d2d17b61f0eb5e4f0](https://github.com/jacobwaller/opendbc/tree/8acbeccdab45a47aa7af170d2d17b61f0eb5e4f0). Jacob authored the VehicleModel angle-limit type correction and integrated the Crosstrek support. The branch also retains the original angle implementation and follow-up work by **lukasloetkolben and elkoled**, alongside comma contributors; their authorship remains theirs. The inherited source covers Subaru angle platform/firmware definitions, `ES_Status` cruise state, angle controller/CAN integration, vehicle-model limits and native angle enforcement. StarPilot has additional platform-specific limits, driver handoff, discovery and AOL safety guards; this attribution does not claim identical tuning or complete parity with the reference fork. Original source commits, author names, emails and titles are recorded in [the Subaru origin history](opendbc_repo/opendbc/car/subaru/ORIGIN_HISTORY.tsv). + +The explicit StarPilot [jacobw Subaru adapter](opendbc_repo/opendbc/car/subaru/JACOBW.md) additionally integrates that branch's low-speed filter, target-first limiting and Crosstrek EPS envelope. Its preserved stricter guards and platform scope are documented alongside the code. diff --git a/opendbc_repo/opendbc/car/subaru/JACOBW.md b/opendbc_repo/opendbc/car/subaru/JACOBW.md new file mode 100644 index 0000000000..efd8e4be20 --- /dev/null +++ b/opendbc_repo/opendbc/car/subaru/JACOBW.md @@ -0,0 +1,23 @@ +# jacobw Subaru angle adapter + +The explicit `jacobw_angle.py` adapter integrates the Crosstrek angle work from +[Jacob Waller's jul-angle-based fork](https://github.com/jacobwaller/opendbc/tree/8acbeccdab45a47aa7af170d2d17b61f0eb5e4f0). +Jacob integrated the implementation and corrected the VehicleModel limit type. +The low-speed filter comes from lukasloetkolben, and target-before-jerk limiting +from elkoled. Original source authors and titles remain in `ORIGIN_HISTORY.tsv`. + +Only `SUBARU_CROSSTREK_2025` selects this adapter, including the Impreza vehicles +identified by that profile. It adds the upstream 6-to-3 degree low-speed +deadzone below 10 m/s, clips the target before jerk limiting, and retains the +190 degree EPS bound. A small native header mirrors that bound for this exact +VM profile. Other Subaru profiles and the shared lateral helper are unchanged. + +StarPilot retains its matched platform vehicle model, the stricter 1 degree +per-command rate instead of upstream's 5, and all existing driver handoff, +MAIN/gear/stop/fault, source-health and AOL permission guards. An impossible +intersection of acceleration and jerk limits withdraws the steering request +and follows bounded measured angle; it cannot relax native limits. Neither +Alpha LONG nor initial AOL arming policy changes. + +This is a StarPilot adaptation, not byte-for-byte upstream tuning. The original +project/opendbc MIT notices and the root source credits apply. diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 24c14a631f..230f49072e 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -5,6 +5,7 @@ from opendbc.car.lateral import (apply_driver_steer_torque_limits, apply_std_ste apply_steer_angle_limits_vm, common_fault_avoidance) from opendbc.car.interfaces import CarControllerBase from opendbc.car.subaru import subarucan +from opendbc.car.subaru.jacobw_angle import limit_angle as jacobw_limit_angle from opendbc.car.subaru.stock_aol import qualified as subaru_aol_qualified from opendbc.car.subaru.values import CAR, DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags from opendbc.car.vehicle_model import VehicleModel @@ -137,6 +138,11 @@ class CarController(CarControllerBase): 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) + elif self.CP.carFingerprint == CAR.SUBARU_CROSSTREK_2025: + assert self.VM is not None + apply_angle, angle_active = jacobw_limit_angle( + actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, + CS.out.steeringAngleDeg, angle_active, self.p, self.VM) else: assert self.VM is not None apply_angle = apply_steer_angle_limits_vm(actuators.steeringAngleDeg, self.apply_angle_last, diff --git a/opendbc_repo/opendbc/car/subaru/jacobw_angle.py b/opendbc_repo/opendbc/car/subaru/jacobw_angle.py new file mode 100644 index 0000000000..9287442a93 --- /dev/null +++ b/opendbc_repo/opendbc/car/subaru/jacobw_angle.py @@ -0,0 +1,31 @@ +"""StarPilot adapter for jacobw's jul-angle-based Crosstrek control. + +Source: jacobwaller/opendbc 8acbeccdab45a47aa7af170d2d17b61f0eb5e4f0. +Low-speed filtering: lukasloetkolben; target-first limiting: elkoled. +Jacob Waller integrated the implementation and corrected its VM limit type. +Keep StarPilot's model, stricter rate, and withdrawal guards. See CREDITS.md. +""" +import numpy as np + +from opendbc.car import rate_limit +from opendbc.car.lateral import apply_center_deadzone, get_max_angle_delta_vm, get_max_angle_vm + +MAX_ANGLE = 190.0 + + +def limit_angle(target, previous, speed, measured, active, limits, model): + if not active: + return float(np.clip(measured, -MAX_ANGLE, MAX_ANGLE)), False + if speed < 10.0: + deadzone = float(np.interp(speed, [2.0, 10.0], [6.0, 3.0])) + target = previous + apply_center_deadzone(target - previous, deadzone) + speed = max(speed, 1.0) + maximum = min(MAX_ANGLE, get_max_angle_vm(speed, model, limits)) + delta = min(get_max_angle_delta_vm(speed, model, limits), limits.ANGLE_LIMITS.MAX_ANGLE_RATE) + # Clip the target before applying jerk limits, as in the source fork. + target = float(np.clip(target, -maximum, maximum)) + # An abrupt input change can leave no intersection between accel and jerk + # limits. Withdraw instead of publishing an active command native must reject. + if previous - delta > maximum or previous + delta < -maximum: + return float(np.clip(measured, -MAX_ANGLE, MAX_ANGLE)), False + return float(np.clip(rate_limit(target, previous, -delta, delta), -MAX_ANGLE, MAX_ANGLE)), True diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_angle_handoff.py b/opendbc_repo/opendbc/car/subaru/tests/test_angle_handoff.py index b21c746fb0..864f1837c1 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_angle_handoff.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_angle_handoff.py @@ -76,9 +76,10 @@ class TestIndependentAngleEnvelope(unittest.TestCase): _, decoded = step(controller, command, state) self.assertEqual(bool(decoded['LKAS_Request']), expected) if not expected: - self.assertAlmostEqual(decoded['LKAS_Output'], angle, places=2) + expected_angle = max(-190., min(190., angle)) if car == CAR.SUBARU_CROSSTREK_2025 else angle + self.assertAlmostEqual(decoded['LKAS_Output'], expected_angle, places=2) - def test_ordinary_engagement_keeps_existing_angle_behavior(self): + def test_ordinary_engagement_respects_crosstrek_bound_and_preserves_other_profiles(self): for car in (CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023, CAR.SUBARU_FORESTER_2022, CAR.SUBARU_OUTBACK_2023): for speed, angle in ((5., 200.), (.4, 10.)): @@ -90,4 +91,7 @@ class TestIndependentAngleEnvelope(unittest.TestCase): state.out.standstill = False for _ in range(3): _, decoded = step(controller, command, state) - self.assertTrue(decoded['LKAS_Request']) + expected = car != CAR.SUBARU_CROSSTREK_2025 or abs(angle) <= 190. + self.assertEqual(bool(decoded['LKAS_Request']), expected) + if not expected: + self.assertEqual(abs(decoded['LKAS_Output']), 190.) diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_jacobw_angle.py b/opendbc_repo/opendbc/car/subaru/tests/test_jacobw_angle.py new file mode 100644 index 0000000000..b04759ebdf --- /dev/null +++ b/opendbc_repo/opendbc/car/subaru/tests/test_jacobw_angle.py @@ -0,0 +1,133 @@ +"""Fork-specific jacobw behavior through physical parsers and real native TX.""" +import pytest + +from opendbc.can import CANParser +from opendbc.car import Bus +from opendbc.car.lateral import get_max_angle_delta_vm, get_max_angle_vm +from opendbc.car.subaru.jacobw_angle import limit_angle +from opendbc.car.subaru.interface import CarInterface +from opendbc.car.subaru.tests.test_ascent_angle import setup_controller, step +from opendbc.car.subaru.values import CAR, DBC +from opendbc.safety.tests.libsafety import libsafety_py +from openpilot.starpilot.car.subaru.tests.caller_helpers import CompleteCan + + +@pytest.mark.parametrize('speed,deadzone', ((1.0, 6.0), (2.0, 6.0), (6.0, 4.5), (9.0, 3.375))) +@pytest.mark.parametrize('sign', (-1, 1)) +def test_real_controller_suppresses_small_low_speed_changes(speed, deadzone, sign): + _, controller, command, state = setup_controller(CAR.SUBARU_CROSSTREK_2025) + state.out.vEgoRaw = speed + command.actuators.steeringAngleDeg = sign * (deadzone - .01) + _, decoded = step(controller, command, state) + assert decoded['LKAS_Request'] == 1 + assert decoded['LKAS_Output'] == 0 + command.actuators.steeringAngleDeg = sign * (deadzone + .01) + _, decoded = step(controller, command, state) + assert decoded['LKAS_Request'] == 1 + assert 0 < sign * decoded['LKAS_Output'] <= 1.0 + + +def test_target_first_limits_recover_to_moving_acceleration_bound(): + _, controller, _, _ = setup_controller(CAR.SUBARU_CROSSTREK_2025) + speed = 30.0 + maximum = get_max_angle_vm(speed, controller.VM, controller.p) + delta = min(get_max_angle_delta_vm(speed, controller.VM, controller.p), 1.0) + previous = maximum + .5 * delta + angle, active = limit_angle(400., previous, speed, 0., True, controller.p, controller.VM) + assert active + assert angle == pytest.approx(maximum) + assert abs(angle - previous) <= delta + + +def test_impossible_accel_jerk_intersection_withdraws(): + _, controller, _, _ = setup_controller(CAR.SUBARU_CROSSTREK_2025) + angle, active = limit_angle(400., 180., 35., 220., True, controller.p, controller.VM) + assert not active and angle == 190. + + +@pytest.mark.parametrize('sign', (-1, 1)) +def test_current_parser_controller_and_native_accept_speed_sweep_and_bound(sign): + cp, controller, command, state = setup_controller(CAR.SUBARU_CROSSTREK_2025) + interface = CarInterface(cp) + controller, state = interface.CC, interface.CS + stream = CompleteCan(interface) + parsers = interface.can_parsers + state.update(parsers) + decoder = CANParser(DBC[cp.carFingerprint][Bus.pt], [('ES_LKAS_ANGLE', 0)], 0) + safety = libsafety_py.libsafety + safety.set_alternative_experience(0) + assert safety.set_safety_hooks(cp.safetyConfigs[0].safetyModel.raw, cp.safetyConfigs[0].safetyParam) == 0 + safety.init_tests() + accepted = 0 + measured = 0. + peak = 0. + try: + for tick in range(2000): + # Stay low long enough to hit the absolute bound, then accelerate and + # decelerate with small physical steps while reversing the target. + speed = 2.0 if tick < 500 else max(2.0, 2.0 + 0.045 * min(tick - 500, 2000 - tick)) + now = 1_000_000_000 + tick * 10_000_000 + frames = stream.frames({ + 'Steering_2': {'Steering_Angle': measured}, + 'Wheel_Speeds': dict.fromkeys(('FL', 'FR', 'RL', 'RR'), speed * 3.6), + 'CruiseControl': {'Cruise_On': 1, 'Cruise_Activated': 1}, + 'ES_Status': {'Cruise_Activated': 1}, 'ES_DashStatus': {'Cruise_On': 1}, + 'Transmission': {'Gear': 121}, + }, tick=tick) + safety.set_timer(now // 1000) + for frame in frames: + assert safety.safety_rx_hook(libsafety_py.make_CANPacket(frame[0], frame[2], frame[1])) + safety.safety_tick() + for parser in parsers.values(): + parser.update([(now, frames)]) + state.out = state.update(parsers) + command.actuators.steeringAngleDeg = sign * (400. if tick < 900 else -400.) + _, sends = controller.update(command.as_reader(), state, now) + for frame in sends: + if frame[0] != 0x124: + continue + decoder.update([(now, [frame])]) + values = decoder.vl['ES_LKAS_ANGLE'] + assert abs(values['LKAS_Output']) <= 190.0 + assert safety.safety_tx_hook(libsafety_py.make_CANPacket(frame[0], frame[2], frame[1])), (tick, speed, values) + accepted += 1 + measured = values['LKAS_Output'] + peak = max(peak, abs(measured)) + assert accepted == 1000 and peak == 190. + # An independently packed out-of-envelope command is rejected natively. + invalid = controller.packer.make_can_msg('ES_LKAS_ANGLE', 0, {'LKAS_Output': sign * 190.01, 'LKAS_Request': 1, 'SET_3': 3}) + assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(invalid[0], invalid[2], invalid[1])) + finally: + safety.set_alternative_experience(0) + safety.set_safety_hooks(0, 0) + safety.init_tests() + + +@pytest.mark.parametrize('sign', (-1, 1)) +def test_native_absolute_bound_independent_of_rate_and_accel(sign): + from opendbc.car.subaru import subarucan + from opendbc.safety.tests.test_subaru_gen2_angle_pair import TestSubaruGen2AnglePair as Pair + + cp = Pair.params(CAR.SUBARU_CROSSTREK_2025) + packer, frames = Pair.sources(CAR.SUBARU_CROSSTREK_2025, speed=7.2) + frames = [packer.make_can_msg('Steering_2', 0, {'Steering_Angle': sign * 190.}) if frame[0] == 0x11a else frame + for frame in frames] + safety = libsafety_py.libsafety + safety.set_alternative_experience(0) + safety.set_safety_hooks(cp.safetyConfigs[0].safetyModel.raw, cp.safetyConfigs[0].safetyParam) + safety.init_tests() + safety.set_timer(1_000_000) + try: + for frame in frames: + assert safety.safety_rx_hook(Pair.packet(frame)) + safety.safety_tick() + assert safety.safety_config_valid() and safety.get_controls_allowed() + # Establish history from a genuine measured-angle inactive frame. At 2m/s, + # accel permits this angle; the final .01deg step is below the jerk bound. + assert safety.safety_tx_hook(Pair.packet(subarucan.create_steering_control_angle(packer, sign * 190., False, 0))) + assert safety.safety_tx_hook(Pair.packet(subarucan.create_steering_control_angle(packer, sign * 190., True, 0))) + assert not safety.safety_tx_hook(Pair.packet(subarucan.create_steering_control_angle(packer, sign * 190.01, True, 0))) + finally: + safety.set_alternative_experience(0) + safety.set_safety_hooks(0, 0) + safety.init_tests() diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_output_contract.py b/opendbc_repo/opendbc/car/subaru/tests/test_output_contract.py index 7109e86131..0bfb314b7b 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_output_contract.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_output_contract.py @@ -7,7 +7,7 @@ from opendbc.car import Bus, structs from opendbc.car.subaru import subarucan from opendbc.car.subaru.stock_aol import ANGLE_CARS, TORQUE_CARS from opendbc.car.subaru.tests.test_ascent_angle import setup_controller, step -from opendbc.car.subaru.values import DBC, SubaruSafetyFlags +from opendbc.car.subaru.values import CAR, DBC, SubaruSafetyFlags from opendbc.safety.tests import test_subaru_gen2_angle_pair as pair from opendbc.safety.tests.libsafety import libsafety_py @@ -18,6 +18,8 @@ from opendbc.safety.tests.libsafety import libsafety_py def test_inactive_sensor_angle_is_bounded_and_native_accepts(car, sign, position): cp, controller, command, state = setup_controller(car) limit = 545.0 if cp.safetyConfigs[0].safetyParam & SubaruSafetyFlags.FIXED_ANGLE_LIMITS else 650.0 + if car == CAR.SUBARU_CROSSTREK_2025: + limit = 190.0 measured = sign * {'within': 100.0, 'limit': limit, 'beyond': limit + 5.0}[position] packer, frames = pair.TestSubaruGen2AnglePair.sources(car, cruise=False) measurement = packer.make_can_msg('Steering_2', 0, {'Steering_Angle': measured}) diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index 249438a154..2f50f3381d 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -6,6 +6,7 @@ #include "opendbc/safety/modes/subaru_sng.h" #include "opendbc/safety/modes/subaru_stop_start.h" #include "opendbc/safety/modes/subaru_avh.h" +#include "opendbc/safety/modes/subaru_jacobw.h" #define SUBARU_STEERING_LIMITS_GENERATOR(steer_max, rate_up, rate_down) \ { \ @@ -183,7 +184,7 @@ 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, + .max_angle = (subaru_forester_angle ? 650 : SUBARU_JACOBW_MAX_ANGLE) * 100, .angle_deg_to_can = 100., .frequency = 50U, }; @@ -252,6 +253,9 @@ static bool subaru_tx_hook(const CANPacket_t *msg) { if (subaru_fixed_angle_limits) { violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_FIXED_ANGLE_STEERING_LIMITS); } else { + if (!subaru_forester_angle) { + violation |= SAFETY_ABS(desired_angle) > SUBARU_ANGLE_STEERING_LIMITS.max_angle; + } violation |= steer_angle_cmd_checks_vm(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS, subaru_forester_angle ? SUBARU_FORESTER_2022_ANGLE_PARAMS : SUBARU_CROSSTREK_2025_ANGLE_PARAMS); } } diff --git a/opendbc_repo/opendbc/safety/modes/subaru_jacobw.h b/opendbc_repo/opendbc/safety/modes/subaru_jacobw.h new file mode 100644 index 0000000000..d6ed9e0304 --- /dev/null +++ b/opendbc_repo/opendbc/safety/modes/subaru_jacobw.h @@ -0,0 +1,6 @@ +#pragma once + +// jacobwaller/opendbc 8acbeccdab45a47aa7af170d2d17b61f0eb5e4f0: +// Crosstrek EPS envelope from lukasloetkolben's angle implementation, integrated +// by Jacob Waller. StarPilot retains its platform-specific VM and rate guards. +#define SUBARU_JACOBW_MAX_ANGLE 190 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 index 429031e2d1..cb9cf396f4 100644 --- a/opendbc_repo/opendbc/safety/tests/test_subaru_gen2_angle_pair.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru_gen2_angle_pair.py @@ -67,7 +67,7 @@ class TestSubaruGen2AnglePair(unittest.TestCase): command = structs.CarControl() command.enabled = True command.latActive = active - command.actuators.steeringAngleDeg = 1.0 + command.actuators.steeringAngleDeg = 10.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], 0) @@ -200,7 +200,7 @@ class TestSubaruGen2AnglePair(unittest.TestCase): # cutting a request on persistent driver input. command = structs.CarControl() command.enabled = command.latActive = True - command.actuators.steeringAngleDeg = 1.0 + command.actuators.steeringAngleDeg = 10.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) diff --git a/prebuilt.json b/prebuilt.json index 6e1d45d044..d0dae4a7ed 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "af5e3a835fba4429405da7e9b6c318ceb40aaa118d5708e62e00d4c947a5845b", "schema_version": 1} +{"protected_inventory_sha256": "531749a5b47f01396f7541b605dcde9ebd839aee9e7eed2b8e3f33e670245031", "schema_version": 1}