Adapt jacobw Subaru angle control to StarPilot

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.

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.

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,

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

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.)

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()

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})

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);
       }
     }

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

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)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "af5e3a835fba4429405da7e9b6c318ceb40aaa118d5708e62e00d4c947a5845b", "schema_version": 1}
+{"protected_inventory_sha256": "531749a5b47f01396f7541b605dcde9ebd839aee9e7eed2b8e3f33e670245031", "schema_version": 1}