Add traffic border color to C4

Cherry-picked corrected Feature Request 510203 implementation from 8768e08341f2d4044776f5ba846e606dc9cd3f1d.

Co-authored-by: Prabhaav Pillai <prabhaav@msu.edu>

common/params_keys.h

@@ -276,7 +276,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DeveloperSidebarMetric7", {PERSISTENT, INT, "7", "0", 3}},
     {"DeveloperUI", {PERSISTENT, BOOL, "0", "0", 3}},
     {"GalaxyDeveloperMode", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
-    {"TestModelLeadTrajectory", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
     {"DeveloperWidgets", {PERSISTENT, BOOL, "1", "0", 3}},
     {"DeviceManagement", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
     {"DeviceShutdown", {PERSISTENT, INT, "6", "6", 1, SETTINGS_SIMPLE}},

opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py

@@ -706,30 +706,6 @@ class TestHyundaiFingerprint:
     assert combined_safety_param & HyundaiSafetyFlags.LONG
     assert combined_safety_param & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE
 
-  def test_sonata_hybrid_aol_main_lkas_sync_is_scoped(self):
-    toggles = SimpleNamespace(always_on_lateral_lkas=True, main_cruise_aol_toggle=True)
-
-    sonata_hybrid_cp = CarInterface.get_params(CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], False, False, False, None)
-    sonata_hybrid_fpcp = CarInterface.get_starpilot_params(
-      CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], sonata_hybrid_cp, toggles,
-    )
-    assert sonata_hybrid_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC
-
-    sonata_cp = CarInterface.get_params(CAR.HYUNDAI_SONATA, gen_empty_fingerprint(), [], False, False, False, None)
-    sonata_fpcp = CarInterface.get_starpilot_params(CAR.HYUNDAI_SONATA, gen_empty_fingerprint(), [], sonata_cp, toggles)
-    assert not (sonata_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC)
-
-    disabled_toggles = SimpleNamespace(always_on_lateral_lkas=True, main_cruise_aol_toggle=False)
-    disabled_fpcp = CarInterface.get_starpilot_params(
-      CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], sonata_hybrid_cp, disabled_toggles,
-    )
-    assert not (disabled_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC)
-
-    minimal_fpcp = CarInterface.get_starpilot_params(
-      CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], sonata_hybrid_cp, SimpleNamespace(),
-    )
-    assert not (minimal_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC)
-
   @pytest.mark.parametrize("candidate", (CAR.HYUNDAI_ELANTRA_2021, CAR.HYUNDAI_SONATA_HYBRID))
   def test_legacy_hyundai_long_does_not_gate_availability_on_main_cruise(self, candidate):
     toggles = get_test_toggles()

opendbc_repo/opendbc/car/hyundai/values.py

@@ -111,7 +111,6 @@ class HyundaiSafetyFlags(IntFlag):
 
 
 class HyundaiStarPilotSafetyFlags(IntFlag):
-  AOL_MAIN_LKAS_SYNC = 32
   HAS_LDA_BUTTON = 1024
   AOL_LKAS_ON_ENGAGE = 2048
 

opendbc_repo/opendbc/car/interfaces.py

@@ -261,9 +261,6 @@ class CarInterfaceBase(ABC):
         if params.get_bool("AlwaysOnLateral") and params.get_int("LKASButtonControl") == 9:
           fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value
 
-        if candidate == HYUNDAI.HYUNDAI_SONATA_HYBRID and getattr(starpilot_toggles, "always_on_lateral_lkas", False) and \
-            getattr(starpilot_toggles, "main_cruise_aol_toggle", False):
-          fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC.value
       elif platform in TOYOTA:
         fp_ret.canUsePedal = not CP.autoResumeSng
         fp_ret.canUseSDSU = candidate not in UNSUPPORTED_DSU_CAR and candidate not in TSS2_CAR
@@ -274,7 +271,7 @@ class CarInterfaceBase(ABC):
         if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR):
           fp_ret.flags |= ToyotaStarPilotFlags.SMART_DSU.value
 
-        if candidate == TOYOTA.TOYOTA_PRIUS:
+        if candidate in (TOYOTA.TOYOTA_PRIUS, TOYOTA.TOYOTA_PRIUS_RETROFIT):
           if 0x23 in fingerprint[0]:
             fp_ret.flags |= ToyotaStarPilotFlags.ZSS.value
 

opendbc_repo/opendbc/car/subaru/carcontroller.py

@@ -22,14 +22,14 @@ _LEGACY_2025_REENGAGE_MAX_STEER_RATE = 2.0
 _LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA = 1.0
 _LEGACY_2025_RECLAIM_FRAMES = 36
 _LEGACY_2025_RECLAIM_EXPONENT = 2.5
-_ASCENT_OVERRIDE_HOLD_FRAMES = 10
-_ASCENT_REENGAGE_SETTLE_FRAMES = 8
-_ASCENT_REENGAGE_MAX_STEER_RATE = 2.0
-_ASCENT_REENGAGE_MAX_ANGLE_DELTA = 1.0
-_ASCENT_RECLAIM_FRAMES = 36
-_ASCENT_RECLAIM_EXPONENT = 2.5
-_ASCENT_MADS_MIN_SPEED = 0.44704
-_ASCENT_MADS_MAX_STEER_ANGLE = 120.0
+_ANGLE_OVERRIDE_HOLD_FRAMES = 10
+_ANGLE_REENGAGE_SETTLE_FRAMES = 8
+_ANGLE_REENGAGE_MAX_STEER_RATE = 2.0
+_ANGLE_REENGAGE_MAX_ANGLE_DELTA = 1.0
+_ANGLE_RECLAIM_FRAMES = 36
+_ANGLE_RECLAIM_EXPONENT = 2.5
+_ANGLE_MADS_MIN_SPEED = 0.44704
+_ANGLE_MADS_MAX_STEER_ANGLE = 120.0
 
 
 def get_safety_CP():
@@ -50,13 +50,13 @@ class CarController(CarControllerBase):
     self.legacy_2025_reengage_reference_angle = 0.0
     self.legacy_2025_reclaim_frames = 0
     self.legacy_2025_reclaim_start_angle = 0.0
-    self.ascent_lkas_active = False
-    self.ascent_handoff_active = False
-    self.ascent_override_hold_frames = 0
-    self.ascent_reengage_settle_frames = 0
-    self.ascent_reengage_reference_angle = 0.0
-    self.ascent_reclaim_frames = 0
-    self.ascent_reclaim_start_angle = 0.0
+    self.angle_lkas_active = False
+    self.angle_handoff_active = False
+    self.angle_override_hold_frames = 0
+    self.angle_reengage_settle_frames = 0
+    self.angle_reengage_reference_angle = 0.0
+    self.angle_reclaim_frames = 0
+    self.angle_reclaim_start_angle = 0.0
 
     self.cruise_button_prev = 0
     self.steer_rate_counter = 0
@@ -137,67 +137,67 @@ class CarController(CarControllerBase):
     self.legacy_2025_reclaim_frames -= 1
     return target_angle
 
-  def _reset_ascent_handoff(self):
-    self.ascent_handoff_active = False
-    self.ascent_override_hold_frames = 0
-    self.ascent_reengage_settle_frames = 0
-    self.ascent_reengage_reference_angle = 0.0
-    self.ascent_reclaim_frames = 0
-    self.ascent_reclaim_start_angle = 0.0
+  def _reset_angle_handoff(self):
+    self.angle_handoff_active = False
+    self.angle_override_hold_frames = 0
+    self.angle_reengage_settle_frames = 0
+    self.angle_reengage_reference_angle = 0.0
+    self.angle_reclaim_frames = 0
+    self.angle_reclaim_start_angle = 0.0
 
-  def _ascent_manual_handoff(self, CS, lat_active):
+  def _angle_manual_handoff(self, CS, lat_active):
     if not lat_active:
-      self._reset_ascent_handoff()
+      self._reset_angle_handoff()
       return False
 
     if CS.out.steeringPressed:
-      self.ascent_handoff_active = True
-      self.ascent_override_hold_frames = _ASCENT_OVERRIDE_HOLD_FRAMES
-      self.ascent_reengage_settle_frames = 0
-      self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
-      self.ascent_reclaim_frames = 0
+      self.angle_handoff_active = True
+      self.angle_override_hold_frames = _ANGLE_OVERRIDE_HOLD_FRAMES
+      self.angle_reengage_settle_frames = 0
+      self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
+      self.angle_reclaim_frames = 0
       return True
 
-    if not self.ascent_handoff_active and not self.ascent_lkas_active and \
-       abs(CS.out.steeringRateDeg) > _ASCENT_REENGAGE_MAX_STEER_RATE:
-      self.ascent_handoff_active = True
-      self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
+    if not self.angle_handoff_active and not self.angle_lkas_active and \
+       abs(CS.out.steeringRateDeg) > _ANGLE_REENGAGE_MAX_STEER_RATE:
+      self.angle_handoff_active = True
+      self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
 
-    if not self.ascent_handoff_active:
+    if not self.angle_handoff_active:
       return False
 
-    if self.ascent_override_hold_frames > 0:
-      self.ascent_override_hold_frames -= 1
-      if self.ascent_override_hold_frames == 0:
-        self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
+    if self.angle_override_hold_frames > 0:
+      self.angle_override_hold_frames -= 1
+      if self.angle_override_hold_frames == 0:
+        self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
       return True
 
-    wheel_stable = abs(CS.out.steeringRateDeg) <= _ASCENT_REENGAGE_MAX_STEER_RATE and \
-      abs(CS.out.steeringAngleDeg - self.ascent_reengage_reference_angle) <= _ASCENT_REENGAGE_MAX_ANGLE_DELTA
+    wheel_stable = abs(CS.out.steeringRateDeg) <= _ANGLE_REENGAGE_MAX_STEER_RATE and \
+      abs(CS.out.steeringAngleDeg - self.angle_reengage_reference_angle) <= _ANGLE_REENGAGE_MAX_ANGLE_DELTA
     if wheel_stable:
-      self.ascent_reengage_settle_frames += 1
+      self.angle_reengage_settle_frames += 1
     else:
-      self.ascent_reengage_settle_frames = 0
-      self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
+      self.angle_reengage_settle_frames = 0
+      self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
 
-    if self.ascent_reengage_settle_frames < _ASCENT_REENGAGE_SETTLE_FRAMES:
+    if self.angle_reengage_settle_frames < _ANGLE_REENGAGE_SETTLE_FRAMES:
       return True
 
-    self.ascent_handoff_active = False
-    self.ascent_reengage_settle_frames = 0
-    self.ascent_reclaim_frames = _ASCENT_RECLAIM_FRAMES
-    self.ascent_reclaim_start_angle = CS.out.steeringAngleDeg
+    self.angle_handoff_active = False
+    self.angle_reengage_settle_frames = 0
+    self.angle_reclaim_frames = _ANGLE_RECLAIM_FRAMES
+    self.angle_reclaim_start_angle = CS.out.steeringAngleDeg
     return True
 
-  def _ascent_reclaim_target(self, target_angle):
-    if self.ascent_reclaim_frames <= 0:
+  def _angle_reclaim_target(self, target_angle):
+    if self.angle_reclaim_frames <= 0:
       return target_angle
 
-    progress = (_ASCENT_RECLAIM_FRAMES - self.ascent_reclaim_frames + 1) / _ASCENT_RECLAIM_FRAMES
-    eased_progress = progress ** _ASCENT_RECLAIM_EXPONENT
-    target_angle = self.ascent_reclaim_start_angle + eased_progress * \
-      (target_angle - self.ascent_reclaim_start_angle)
-    self.ascent_reclaim_frames -= 1
+    progress = (_ANGLE_RECLAIM_FRAMES - self.angle_reclaim_frames + 1) / _ANGLE_RECLAIM_FRAMES
+    eased_progress = progress ** _ANGLE_RECLAIM_EXPONENT
+    target_angle = self.angle_reclaim_start_angle + eased_progress * \
+      (target_angle - self.angle_reclaim_start_angle)
+    self.angle_reclaim_frames -= 1
     return target_angle
 
   def lateral_angle(self, CC, CS):
@@ -224,30 +224,41 @@ class CarController(CarControllerBase):
       self.legacy_2025_lkas_active = lkas_active
       return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
 
-    if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
+    if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
       mads_only = CC.latActive and not CC.enabled
-      mads_only_ok = CS.out.vEgoRaw > _ASCENT_MADS_MIN_SPEED and \
-        abs(CS.out.steeringAngleDeg) < _ASCENT_MADS_MAX_STEER_ANGLE
+      mads_only_ok = CS.out.vEgoRaw > _ANGLE_MADS_MIN_SPEED and \
+        abs(CS.out.steeringAngleDeg) < _ANGLE_MADS_MAX_STEER_ANGLE
       lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
         CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
 
-      manual_handoff = self._ascent_manual_handoff(CS, lkas_available)
+      manual_handoff = self._angle_manual_handoff(CS, lkas_available)
       lkas_active = lkas_available and not manual_handoff
 
-      if lkas_active and not self.ascent_lkas_active:
+      if lkas_active and not self.angle_lkas_active:
         self.apply_steer_last = CS.out.steeringAngleDeg
 
-      steer_target = self._ascent_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
-      apply_steer = apply_std_steer_angle_limits(
-        steer_target,
-        self.apply_steer_last,
-        CS.out.vEgoRaw,
-        CS.out.steeringAngleDeg,
-        lkas_active,
-        self.p.FIXED_ANGLE_LIMITS,
-      )
+      steer_target = self._angle_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
+      if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
+        apply_steer = apply_std_steer_angle_limits(
+          steer_target,
+          self.apply_steer_last,
+          CS.out.vEgoRaw,
+          CS.out.steeringAngleDeg,
+          lkas_active,
+          self.p.FIXED_ANGLE_LIMITS,
+        )
+      else:
+        apply_steer = apply_steer_angle_limits_vm(
+          steer_target,
+          self.apply_steer_last,
+          CS.out.vEgoRaw,
+          CS.out.steeringAngleDeg,
+          lkas_active,
+          self.p,
+          self.VM,
+        )
       self.apply_steer_last = apply_steer
-      self.ascent_lkas_active = lkas_active
+      self.angle_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)

opendbc_repo/opendbc/car/subaru/interface.py

@@ -51,6 +51,8 @@ class CarInterface(CarInterfaceBase):
 
     if ret.flags & SubaruFlags.LKAS_ANGLE:
       ret.steerControlType = structs.CarParams.SteerControlType.angle
+      if candidate == CAR.SUBARU_OUTBACK_2023:
+        ret.lateralSmoothSeconds = 0.4
 
     elif candidate in (CAR.SUBARU_ASCENT, CAR.SUBARU_ASCENT_2023):
       ret.steerActuatorDelay = 0.3  # end-to-end angle controller

opendbc_repo/opendbc/car/subaru/tests/test_subaru.py

@@ -202,6 +202,7 @@ def test_outback_2023_uses_d_platform_bus_layout():
   assert parsers[Bus.main].bus == CanBus.main
   assert controller.angle_bus == CanBus.main
   assert controller.status_bus == CanBus.main
+  assert CP.lateralSmoothSeconds == pytest.approx(0.4)
 
 
 def test_legacy_2025_uses_gen2_angle_bus_layout():
@@ -458,8 +459,9 @@ def test_ascent_angle_controller_uses_fixed_angle_rate_limits():
   assert CS.out.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < -25.0
 
 
-def test_ascent_angle_controller_yields_until_manual_steering_settles():
-  CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
+@pytest.mark.parametrize("platform", (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023))
+def test_angle_controller_yields_until_manual_steering_settles(platform):
+  CP = CarInterface.get_non_essential_params(platform)
   controller = CarController({}, CP)
   CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-10.0))
   CS = SimpleNamespace(out=SimpleNamespace(

opendbc_repo/opendbc/car/tests/test_can_fingerprint.py

@@ -27,7 +27,7 @@ class TestCanFingerprint:
       fingerprint_iter = iter([can])
       car_fingerprint, finger = can_fingerprint(lambda **kwargs: [next(fingerprint_iter, [])])  # noqa: B023
 
-      if car_model == TOYOTA.TOYOTA_MATRIX_RETROFIT:
+      if car_model in (TOYOTA.TOYOTA_MATRIX_RETROFIT, TOYOTA.TOYOTA_PRIUS_RETROFIT):
         assert fingerprint == {}
         assert car_fingerprint is None
       elif car_fingerprint is None and str(car_model).startswith(("BUICK_", "CADILLAC_", "CHEVROLET_", "GMC_", "HOLDEN_")):

opendbc_repo/opendbc/car/torque_data/substitute.toml

@@ -9,6 +9,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 
 "TOYOTA_ALPHARD_TSS2" = "TOYOTA_SIENNA"
 "TOYOTA_PRIUS_V" = "TOYOTA_PRIUS"
+"TOYOTA_PRIUS_RETROFIT" = "TOYOTA_PRIUS"
 "TOYOTA_SIENNA_4TH_GEN" = "TOYOTA_RAV4_PRIME"
 "LEXUS_IS" = "LEXUS_NX"
 "LEXUS_CTH" = "LEXUS_NX"

opendbc_repo/opendbc/car/toyota/carcontroller.py

@@ -11,7 +11,7 @@ from opendbc.car.interfaces import CarControllerBase
 from opendbc.car.toyota import toyotacan
 from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, NO_STOP_TIMER_CAR, PEDAL_TRANSITION, TSS2_CAR, \
                                         CarControllerParams, ToyotaFlags, \
-                                        UNSUPPORTED_DSU_CAR
+                                        UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR
 from opendbc.can import CANPacker
 
 Ecu = structs.CarParams.Ecu
@@ -59,13 +59,17 @@ def is_camry_hybrid(CP) -> bool:
 
 
 def is_ths_hybrid(CP) -> bool:
-  return CP.carFingerprint == CAR.TOYOTA_PRIUS or is_camry_hybrid(CP)
+  return CP.carFingerprint in LEGACY_PRIUS_CAR or is_camry_hybrid(CP)
 
 
-def should_bypass_toyota_long_pid(CP) -> bool:
+def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
+  highlander_sdsu = (
+    CP.carFingerprint == CAR.TOYOTA_HIGHLANDER and
+    bool(getattr(starpilot_toggles, "has_sdsu", False))
+  )
   return bool(CP.enableGasInterceptorDEPRECATED or (
     CP.carFingerprint == CAR.TOYOTA_CAMRY and not is_camry_hybrid(CP)
-  ))
+  ) or highlander_sdsu)
 
 
 def get_long_tune(CP, params):
@@ -74,7 +78,7 @@ def get_long_tune(CP, params):
   k_f = 1.0
 
   if is_ths_hybrid(CP):
-    k_f = 0.8 if CP.carFingerprint == CAR.TOYOTA_PRIUS else 1.0
+    k_f = 0.8 if CP.carFingerprint in LEGACY_PRIUS_CAR else 1.0
   elif CP.carFingerprint not in TSS2_CAR:
     kiBP = [0., 5., 35.]
     kiV = [3.6, 2.4, 1.5]
@@ -454,7 +458,7 @@ class CarController(CarControllerBase):
         a_ego_future = a_ego_blended + j_ego * future_t
 
         if CC.longActive:
-          if should_bypass_toyota_long_pid(self.CP):
+          if should_bypass_toyota_long_pid(self.CP, starpilot_toggles):
             # Pedal/SDSU Toyotas have shown better behavior when we trust the planner
             # target directly instead of letting the Toyota longitudinal PID swing it
             # around. Keep the shared rate limits above, but bypass the extra
@@ -477,7 +481,7 @@ class CarController(CarControllerBase):
               pcm_accel_cmd += pitch_compensation
 
             feedforward = pcm_accel_cmd
-            if self.CP.carFingerprint == CAR.TOYOTA_PRIUS:
+            if self.CP.carFingerprint in LEGACY_PRIUS_CAR:
               feedforward = get_prius_feedforward(feedforward, CS.out.vEgo)
             elif is_camry_hybrid(self.CP) and feedforward > 0.0:
               # Preserve the established Camry Hybrid acceleration response while
@@ -507,7 +511,7 @@ class CarController(CarControllerBase):
         else:
           pcm_accel_cmd = limit_no_lead_cruise_sign_flip(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo,
                                                          CS.out.cruiseState.speed, bool(hud_control.leadVisible))
-          if self.CP.carFingerprint == CAR.TOYOTA_PRIUS:
+          if self.CP.carFingerprint in LEGACY_PRIUS_CAR:
             pcm_accel_cmd = limit_prius_stopping_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo, lead)
 
         pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))

opendbc_repo/opendbc/car/toyota/carstate.py

@@ -8,7 +8,7 @@ from opendbc.car.common.filter_simple import FirstOrderFilter
 from opendbc.car.interfaces import CarStateBase
 from opendbc.car.toyota.values import ToyotaFlags, ToyotaStarPilotFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
                                                   TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, \
-                                                  SECOC_CAR
+                                                  SECOC_CAR, LEGACY_PRIUS_CAR
 
 ButtonType = structs.CarState.ButtonEvent.Type
 SteerControlType = structs.CarParams.SteerControlType
@@ -23,7 +23,7 @@ TEMP_STEER_FAULTS = (0, 9, 11, 21, 25)
 # - lka/lta msg drop out: 3 (recoverable)
 # - prolonged high driver torque: 17 (permanent)
 PERM_STEER_FAULTS = (3, 17)
-LKAS_BUTTON_CAR = TSS2_CAR | {CAR.TOYOTA_PRIUS}
+LKAS_BUTTON_CAR = TSS2_CAR | LEGACY_PRIUS_CAR
 DISTANCE_BUTTON_CAR = {CAR.TOYOTA_SIENNA_4TH_GEN}
 
 

opendbc_repo/opendbc/car/toyota/fingerprints.py

@@ -6,6 +6,7 @@ Ecu = CarParams.Ecu
 
 FINGERPRINTS = {
   CAR.TOYOTA_MATRIX_RETROFIT: [{}],
+  CAR.TOYOTA_PRIUS_RETROFIT: [{}],
 }
 
 FW_VERSIONS = {

opendbc_repo/opendbc/car/toyota/interface.py

@@ -4,7 +4,7 @@ from opendbc.car.toyota.carcontroller import CarController
 from opendbc.car.toyota.radar_interface import RadarInterface
 from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, SECOC_CAR, NO_DSU_CAR, \
                                                   MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \
-                                                  ToyotaSafetyFlags
+                                                  ToyotaSafetyFlags, LEGACY_PRIUS_CAR
 from opendbc.car.disable_ecu import disable_ecu
 from opendbc.car.interfaces import CarInterfaceBase
 from opendbc.safety import ALTERNATIVE_EXPERIENCE
@@ -67,7 +67,7 @@ class CarInterface(CarInterfaceBase):
     # These messages are normally absent there on pre-TSS2 platforms.
     camera_fingerprint = fingerprint.get(2, {})
     has_dsu_bypass = 0x343 in camera_fingerprint or 0x4CB in camera_fingerprint
-    late_prius_camera = candidate == CAR.TOYOTA_PRIUS and any(
+    late_prius_camera = candidate in LEGACY_PRIUS_CAR and any(
       fw.ecu == Ecu.fwdCamera and bytes(fw.fwVersion).startswith(b'8646F4705') for fw in car_fw
     )
     if candidate in (CAR.LEXUS_IS, CAR.TOYOTA_CAMRY) or late_prius_camera:
@@ -83,7 +83,7 @@ class CarInterface(CarInterfaceBase):
     if Ecu.hybrid in found_ecus:
       ret.flags |= ToyotaFlags.HYBRID.value
 
-    if candidate == CAR.TOYOTA_PRIUS:
+    if candidate in LEGACY_PRIUS_CAR:
       stop_and_go = True
       ret.flags |= ToyotaFlags.HYBRID.value
       # Only give steer angle deadzone to for bad angle sensor prius
@@ -175,7 +175,7 @@ class CarInterface(CarInterfaceBase):
     # to a negative value, so it won't matter.
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptorDEPRECATED) else MIN_ACC_SPEED
 
-    prius_long_defaults = candidate == CAR.TOYOTA_PRIUS and ret.openpilotLongitudinalControl
+    prius_long_defaults = candidate in LEGACY_PRIUS_CAR and ret.openpilotLongitudinalControl
     camry_hybrid_long_defaults = (candidate == CAR.TOYOTA_CAMRY and ret.openpilotLongitudinalControl and
                                   bool(ret.flags & ToyotaFlags.HYBRID.value))
 

opendbc_repo/opendbc/car/toyota/tests/test_toyota.py

@@ -75,6 +75,22 @@ class TestToyotaInterfaces:
     assert default_params.lateralTuning.torque.steeringAngleDeadzoneDeg == pytest.approx(0.3)
     assert forced_params.lateralTuning.torque.steeringAngleDeadzoneDeg == pytest.approx(0.3)
 
+  def test_prius_tss2_eps_retrofit_uses_legacy_body_and_eps_scale(self):
+    params = CarInterface.get_params(
+      CAR.TOYOTA_PRIUS_RETROFIT,
+      {bus: {} for bus in range(8)},
+      [],
+      False,
+      False,
+      False,
+      SimpleNamespace(force_torque_controller=False, nnff=False, nnff_lite=False),
+    )
+
+    assert params.lateralTuning.which() == "torque"
+    assert params.safetyConfigs[0].safetyParam & 0xFF == 73
+    assert params.flags & ToyotaFlags.TSS2.value == 0
+    assert params.steerRatio == pytest.approx(15.74)
+
   def test_sienna_4th_gen_uses_torque_controller(self):
     params = CarInterface.get_params(
       CAR.TOYOTA_SIENNA_4TH_GEN,
@@ -469,6 +485,15 @@ class TestToyotaInterfaces:
 
     assert not should_bypass_toyota_long_pid(car_params)
 
+  def test_highlander_sdsu_bypasses_toyota_longitudinal_pid(self):
+    car_params = SimpleNamespace(
+      carFingerprint=CAR.TOYOTA_HIGHLANDER,
+      enableGasInterceptorDEPRECATED=False,
+    )
+
+    assert should_bypass_toyota_long_pid(car_params, SimpleNamespace(has_sdsu=True))
+    assert not should_bypass_toyota_long_pid(car_params, SimpleNamespace(has_sdsu=False))
+
   def test_camry_continental_radar_converts_absolute_target_speed(self):
     radar_interface = RadarInterface.__new__(RadarInterface)
     radar_interface.CP = SimpleNamespace(wheelSpeedFactor=1.0)

opendbc_repo/opendbc/car/toyota/values.py

@@ -250,6 +250,11 @@ class CAR(Platforms):
     CarSpecs(mass=3045. * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.74, tireStiffnessFactor=0.6371),
     dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
   )
+  TOYOTA_PRIUS_RETROFIT = PlatformConfig(
+    [ToyotaCommunityCarDocs("Toyota Prius 2016-20 with TSS2 EPS retrofit", package="Custom retrofit")],
+    TOYOTA_PRIUS.specs,
+    dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
+  )
   TOYOTA_PRIUS_V = PlatformConfig(
     [ToyotaCarDocs("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED)],
     CarSpecs(mass=3340. * CV.LB_TO_KG, wheelbase=2.78, steerRatio=17.4, tireStiffnessFactor=0.5533),
@@ -599,9 +604,11 @@ STEER_THRESHOLD = 100
 
 # These cars have non-standard EPS torque scale factors. All others are 73
 EPS_SCALE = defaultdict(lambda: 73,
-                        {CAR.TOYOTA_PRIUS: 66, CAR.TOYOTA_COROLLA: 88, CAR.TOYOTA_MATRIX_RETROFIT: 88,
+                        {CAR.TOYOTA_PRIUS: 66, CAR.TOYOTA_PRIUS_RETROFIT: 73, CAR.TOYOTA_COROLLA: 88, CAR.TOYOTA_MATRIX_RETROFIT: 88,
                          CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.TOYOTA_PRIUS_V: 100})
 
+LEGACY_PRIUS_CAR = frozenset((CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT))
+
 # Toyota/Lexus Safety Sense 2.0 and 2.5
 TSS2_CAR = CAR.with_flags(ToyotaFlags.TSS2)
 

opendbc_repo/opendbc/safety/modes/hyundai.h

@@ -90,7 +90,6 @@ static const CanMsg HYUNDAI_LONG_REFRESH_TX_MSGS[] = {
 
 static bool hyundai_legacy = false;
 static bool hyundai_can_canfd_blended_hda2 = false;
-static bool hyundai_acc_main_on_rx_prev = false;
 
 #define HYUNDAI_CAN_CANFD_BLENDED_HDA2_COMMON_RX_CHECKS()                                                                                                      \
   {.msg = {{0x260, 1, 8, 100U, .max_counter = 3U, .ignore_quality_flag = true},                                                                                \
@@ -194,12 +193,7 @@ static void hyundai_rx_hook(const CANPacket_t *msg) {
   if (msg->addr == 0x420U) {
     if (msg->bus == scc_bus) {
       if (!hyundai_longitudinal) {
-        const bool acc_main_on_rx = GET_BIT(msg, hyundai_can_canfd_blended ? 27U : 0U);
-        if (hyundai_aol_main_lkas_sync && (acc_main_on_rx != hyundai_acc_main_on_rx_prev)) {
-          lkas_on = false;
-        }
-        acc_main_on = acc_main_on_rx;
-        hyundai_acc_main_on_rx_prev = acc_main_on_rx;
+        acc_main_on = GET_BIT(msg, hyundai_can_canfd_blended ? 27U : 0U);
       }
     }
   }
@@ -448,8 +442,6 @@ static safety_config hyundai_init(uint16_t param) {
   hyundai_common_init(param);
   hyundai_legacy = false;
   hyundai_can_canfd_blended_hda2 = hyundai_can_canfd_blended && hyundai_canfd_lka_steering;
-  hyundai_aol_main_lkas_sync = GET_FLAG(param, 32U);
-  hyundai_acc_main_on_rx_prev = false;
 
   if (hyundai_can_canfd_blended) {
     gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut);

opendbc_repo/opendbc/safety/modes/hyundai_common.h

@@ -60,9 +60,6 @@ bool hyundai_cancel_button_enable = false;
 extern bool hyundai_can_refresh_msgs;
 bool hyundai_can_refresh_msgs = false;
 
-extern bool hyundai_aol_main_lkas_sync;
-bool hyundai_aol_main_lkas_sync = false;
-
 static uint8_t hyundai_last_button_interaction;  // button messages since the user pressed an enable button
 static bool acc_main_on_prev;
 static bool acc_main_on_tx;
@@ -98,7 +95,6 @@ void hyundai_common_init(uint16_t param) {
   hyundai_non_scc = GET_FLAG(param, HYUNDAI_PARAM_NON_SCC);
   hyundai_cancel_button_enable = GET_FLAG(param, HYUNDAI_PARAM_CANCEL_BTN_ENABLE);
   hyundai_can_refresh_msgs = GET_FLAG(param, HYUNDAI_PARAM_CAN_REFRESH_MSGS);
-  hyundai_aol_main_lkas_sync = false;
 
   hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
   acc_main_on_prev = false;
@@ -164,9 +160,7 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
   }
 
   if (main_button && !main_button_prev) {
-    if (!hyundai_aol_main_lkas_sync) {
-      acc_main_on = !acc_main_on;
-    }
+    acc_main_on = !acc_main_on;
   }
   main_button_prev = main_button;
 }

opendbc_repo/opendbc/safety/tests/test_hyundai.py

@@ -4,7 +4,6 @@ import unittest
 
 from opendbc.car.hyundai.values import HyundaiSafetyFlags, HyundaiStarPilotSafetyFlags
 from opendbc.car.structs import CarParams
-from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from opendbc.safety.tests.libsafety import libsafety_py
 import opendbc.safety.tests.common as common
 from opendbc.safety.tests.common import CANPackerSafety
@@ -622,65 +621,5 @@ class TestHyundaiAolLkasOnEngageStockSafety(HyundaiAolLkasOnEngageStockBase, Tes
     self.safety.init_tests()
 
 
-class TestHyundaiAolMainLkasSyncSafety(TestHyundaiSafety):
-  def setUp(self):
-    self.packer = CANPackerSafety("hyundai_kia_generic")
-    self.safety = libsafety_py.libsafety
-    self.safety.set_safety_hooks(
-      CarParams.SafetyModel.hyundai,
-      HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON | HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC,
-    )
-    self.safety.init_tests()
-
-  @staticmethod
-  def _lkas_button_msg(pressed):
-    dat = bytearray(8)
-    dat[0] = int(pressed) << 4
-    return libsafety_py.make_CANPacket(0x391, 0, bytes(dat))
-
-  def test_confirmed_main_state_rephases_lkas_button(self):
-    self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
-    self.safety.set_controls_allowed(False)
-
-    self._rx(self._lkas_button_msg(True))
-    self._rx(self._lkas_button_msg(False))
-    self.assertTrue(self.safety.get_lkas_on())
-    self.assertTrue(self.safety.get_aol_allowed())
-    self._set_prev_torque(0)
-    self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
-
-    self._rx(self._button_msg(Buttons.NONE, main_button=True))
-    self._rx(self._button_msg(Buttons.NONE, main_button=False))
-    self.assertFalse(self.safety.get_acc_main_on())
-    self.assertTrue(self.safety.get_lkas_on())
-    self.assertTrue(self.safety.get_aol_allowed())
-
-    self._rx(self._acc_state_msg(True))
-    self.assertFalse(self.safety.get_lkas_on())
-    self.assertTrue(self.safety.get_aol_allowed())
-    self._set_prev_torque(0)
-    self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
-
-    self._rx(self._button_msg(Buttons.NONE, main_button=True))
-    self._rx(self._button_msg(Buttons.NONE, main_button=False))
-    self.assertTrue(self.safety.get_acc_main_on())
-    self.assertTrue(self.safety.get_aol_allowed())
-
-    self._rx(self._acc_state_msg(False))
-    self.assertFalse(self.safety.get_controls_allowed())
-    self.assertFalse(self.safety.get_acc_main_on())
-    self.assertFalse(self.safety.get_lkas_on())
-    self.assertFalse(self.safety.get_aol_allowed())
-    self._set_prev_torque(0)
-    self.assertFalse(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
-
-    self._rx(self._lkas_button_msg(True))
-    self._rx(self._lkas_button_msg(False))
-    self.assertTrue(self.safety.get_lkas_on())
-    self.assertTrue(self.safety.get_aol_allowed())
-    self._set_prev_torque(0)
-    self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
-
-
 if __name__ == "__main__":
   unittest.main()

panda/board/obj/gitversion.h

@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-4078e6fc-DEBUG";
+const uint8_t gitversion[19] = "DEV-459d7c40-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-4078e6fc-DEBUG
+DEV-459d7c40-DEBUG

selfdrive/controls/controlsd.py

@@ -221,9 +221,9 @@ def get_plan_reach(model_v2) -> float:
 
 
 def get_control_lateral_smooth_seconds(brand: str, v_ego: float, vehicle_smooth_seconds: float) -> float:
-  if brand != "rivian":
-    return LAT_SMOOTH_SECONDS
-  return get_car_lateral_smooth_seconds(brand, v_ego, vehicle_smooth_seconds)
+  if brand == "rivian" or (brand == "subaru" and vehicle_smooth_seconds > 0.0):
+    return get_car_lateral_smooth_seconds(brand, v_ego, vehicle_smooth_seconds)
+  return LAT_SMOOTH_SECONDS
 
 
 def turn_lead_allowed(brand: str, lateral_control_mode: car.CarControl.Actuators.LateralControlMode) -> bool:

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -169,6 +169,7 @@ KONA_NON_SCC_CARS = (
 )
 PRIUS_CARS = (
   TOYOTA_CAR.TOYOTA_PRIUS,
+  TOYOTA_CAR.TOYOTA_PRIUS_RETROFIT,
 )
 
 CAMRY_CARS = (

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -627,11 +627,10 @@ class LongitudinalMpc:
     return lead_xv
 
   def process_lead(self, lead, tracking_lead=True, t_follow=None, *, lead_index=0,
-                   smooth_duplicate_vision=False, model_lead=None,
-                   use_model_lead_trajectory=False):
+                   smooth_duplicate_vision=False, model_lead=None):
     v_ego = self.x0[1]
     lead_active = lead is not None and lead.status and tracking_lead
-    if lead_active and use_model_lead_trajectory:
+    if lead_active:
       model_lead_xv = build_model_lead_trajectory(model_lead, lead, v_ego)
       if model_lead_xv is not None:
         return model_lead_xv
@@ -924,23 +923,18 @@ class LongitudinalMpc:
   def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow,
              personality=log.LongitudinalPersonality.standard, tracking_lead=True,
              optional_far_lead_comfort=True, smooth_duplicate_vision=False,
-             stop_x=None, silverado_early_follow=False, modelV2=None,
-             use_model_lead_trajectory=False):
+             stop_x=None, silverado_early_follow=False, modelV2=None):
     v_ego = self.x0[1]
     lead_one = radarstate.leadOne
     lead_two = radarstate.leadTwo
     self.status = tracking_lead and (lead_one.status or lead_two.status)
-    model_leads = ()
-    if use_model_lead_trajectory and modelV2 is not None:
-      model_leads = getattr(modelV2, "leadsV3", ())
+    model_leads = getattr(modelV2, "leadsV3", ()) if modelV2 is not None else ()
     lead_xv_0 = self.process_lead(lead_one, tracking_lead, t_follow=t_follow, lead_index=0,
                                   smooth_duplicate_vision=smooth_duplicate_vision,
-                                  model_lead=model_leads[0] if len(model_leads) > 0 else None,
-                                  use_model_lead_trajectory=use_model_lead_trajectory)
+                                  model_lead=model_leads[0] if len(model_leads) > 0 else None)
     lead_xv_1 = self.process_lead(lead_two, tracking_lead, t_follow=t_follow, lead_index=1,
                                   smooth_duplicate_vision=smooth_duplicate_vision,
-                                  model_lead=model_leads[1] if len(model_leads) > 1 else None,
-                                  use_model_lead_trajectory=use_model_lead_trajectory)
+                                  model_lead=model_leads[1] if len(model_leads) > 1 else None)
     self.lead_xv_0 = lead_xv_0
     self.lead_xv_1 = lead_xv_1
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -2208,8 +2208,7 @@ class LongitudinalPlanner:
                     smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass,
                     stop_x=force_stop_x,
                     silverado_early_follow=early_truck_follow,
-                    modelV2=sm['modelV2'],
-                    use_model_lead_trajectory=bool(getattr(starpilot_toggles, "test_model_lead_trajectory", False)))
+                    modelV2=sm['modelV2'])
 
     self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -329,20 +329,16 @@ def make_model_lead(*, prob: float = 0.99, x=None, v=None):
   return model, lead
 
 
-def test_model_lead_trajectory_is_opt_in_and_disabled_path_is_unchanged():
+def test_model_lead_trajectory_is_default_for_stable_lead():
   lead = make_lead(status=True, d_rel=42.0, v_lead=18.0, model_prob=0.99)
   _, model_lead = make_model_lead()
 
-  legacy_mpc = LongitudinalMpc()
-  disabled_mpc = LongitudinalMpc()
-  legacy_mpc.set_cur_state(20.0, 0.0)
-  disabled_mpc.set_cur_state(20.0, 0.0)
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(20.0, 0.0)
 
-  legacy = legacy_mpc.process_lead(lead)
-  disabled = disabled_mpc.process_lead(
-    lead, model_lead=model_lead, use_model_lead_trajectory=False,
-  )
-  np.testing.assert_allclose(disabled, legacy)
+  actual = mpc.process_lead(lead, model_lead=model_lead)
+  expected = build_model_lead_trajectory(model_lead, lead, 20.0)
+  np.testing.assert_allclose(actual, expected)
 
 
 def test_model_lead_trajectory_uses_raw_current_anchor_and_future_deltas():

selfdrive/controls/tests/test_starpilot_acceleration.py

@@ -221,14 +221,17 @@ def test_pulse_and_glide_coasts_at_set_speed_then_resumes_below_delta():
 
   accel.update(set_speed, make_sm(set_speed_kph=100.0, pulse_and_glide=True), toggles)
   assert accel.pulse_glide_coasting is True
+  assert accel.pulse_glide_target == pytest.approx(set_speed - delta)
   assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO)
 
   accel.update((90.0 * CV.KPH_TO_MS) - 0.05, make_sm(set_speed_kph=100.0, pulse_and_glide=True), toggles)
   assert accel.pulse_glide_coasting is False
+  assert accel.pulse_glide_target is None
   assert accel.min_accel == pytest.approx(A_CRUISE_MIN)
 
   accel.update(99.8 * CV.KPH_TO_MS, make_sm(set_speed_kph=100.0, pulse_and_glide=True), toggles)
   assert accel.pulse_glide_coasting is True
+  assert accel.pulse_glide_target == pytest.approx(set_speed - delta)
   assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO)
 
 
@@ -239,4 +242,5 @@ def test_pulse_and_glide_is_inert_when_disabled():
   accel.update(100.0 * CV.KPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["STANDARD"], pulse_glide_speed_delta=10.0))
 
   assert accel.pulse_glide_coasting is False
+  assert accel.pulse_glide_target is None
   assert accel.min_accel == pytest.approx(A_CRUISE_MIN)

selfdrive/controls/tests/test_starpilot_planner.py

@@ -2,6 +2,7 @@ import math
 from pathlib import Path
 from types import SimpleNamespace
 
+from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
 from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner, get_force_stop_jerk_scale
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_lead_follow_jerk_scale
@@ -119,6 +120,25 @@ def test_standstill_without_turn_signal_keeps_lateral_allowed(monkeypatch):
     planner.shutdown()
 
 
+def test_pulse_glide_target_is_published_after_vcruise_update(monkeypatch):
+  planner = make_planner(monkeypatch)
+
+  try:
+    normal_target = 65.0 * CV.MPH_TO_MS
+    glide_target = 60.0 * CV.MPH_TO_MS
+    monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: normal_target)
+
+    def publish_glide_target(*args, **kwargs):
+      planner.starpilot_acceleration.pulse_glide_target = glide_target
+
+    monkeypatch.setattr(planner.starpilot_acceleration, "update", publish_glide_target)
+    planner.update(0.0, False, make_sm(planner, frame=1, v_ego=normal_target, left_blinker=False), make_toggles())
+
+    assert planner.v_cruise == glide_target
+  finally:
+    planner.shutdown()
+
+
 def test_lateral_resume_delay_holds_resume_after_low_speed_turn(monkeypatch):
   planner = make_planner(monkeypatch)
 

selfdrive/controls/tests/test_turn_lead.py

@@ -21,6 +21,15 @@ def test_non_rivian_control_smoothing_matches_starpilot(v_ego):
   assert get_control_lateral_smooth_seconds("toyota", v_ego, 0.0) == 0.1
 
 
+@pytest.mark.parametrize(("v_ego", "expected"), [
+  (0.0, 0.4),
+  (5.0, 0.2),
+  (30.0, 0.0),
+])
+def test_subaru_control_smoothing_uses_vehicle_schedule(v_ego, expected):
+  assert get_control_lateral_smooth_seconds("subaru", v_ego, 0.4) == pytest.approx(expected)
+
+
 @pytest.mark.parametrize(("v_ego", "expected"), [
   (0.0, 0.4),
   (5.0, 0.2),