attempt to get sim working

Force Stop is now better at faster approach speeds. It has more authority on braking in general.

Force Stop Distance Offset: still the lever that moves the ending point sooner or later.

Radar bug fixed

common/params_keys.h

@@ -242,6 +242,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"CommunityFavorites", {PERSISTENT, STRING, "", "", 1}},
     {"ConditionalChill", {PERSISTENT, BOOL, "0", "0", 1}},
     {"ConditionalExperimental", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
+    {"HybridExpBias", {PERSISTENT, FLOAT, "0", "0", 1}},
+    {"HybridExperimental", {PERSISTENT, BOOL, "0", "0", 1}},
+    {"HybridVisionBrakeSensitivity", {PERSISTENT, FLOAT, "1", "1", 1}},
+    {"HEMExpDominant", {CLEAR_ON_MANAGER_START, BOOL, "0", "0", 2}},
     {"CurvatureData", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}},
     {"CurveSpeedController", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
     {"CurveSpeedControllerNoLead", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
@@ -522,6 +526,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"PIPPreviewMask", {PERSISTENT, JSON, "{\"width\":1928,\"height\":1208,\"center_left\":[315,548],\"center_right\":[1571,539],\"crop_size\":580}", "{\"width\":1928,\"height\":1208,\"center_left\":[315,548],\"center_right\":[1571,539],\"crop_size\":580}", 2}},
     {"PIPPreviewShowOnBlinker", {PERSISTENT, BOOL, "0", "0", 1}},
     {"PIPPreviewShowOnBSM", {PERSISTENT, BOOL, "0", "0", 1}},
+    {"PIPPreviewInvert", {PERSISTENT, BOOL, "0", "0", 1}},
     {"GalaxyPaired", {PERSISTENT, BOOL, "0", "0", 0}},
     {"GalaxyUploadPending", {PERSISTENT, BOOL, "0", "0", 0}},
     {"PreferredSchedule", {PERSISTENT, INT, "2", "0", 0}},
@@ -664,6 +669,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"SwitchbackModeEnabled", {CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0", "0"}},
     {"SubaruSNG", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
     {"SubaruSNGManualParkingBrake", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
+    {"SubaruStopStartOff", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
     {"TacoTune", {PERSISTENT, BOOL, "0", "0", 2}},
     {"TeslaCoopSteering", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
     {"TestAlert", {CLEAR_ON_MANAGER_START, STRING, "", ""}},

opendbc_repo/opendbc/car/hyundai/carcontroller.py

@@ -180,6 +180,13 @@ def should_use_ev6_gt_line_stop_direct_tracking(ev6_gt_line: bool, stopping: boo
   return bool(ev6_gt_line and stopping and v_ego > EV6_GT_LINE_STOP_BRAKE_CAP_MAX_SPEED and accel_cmd < actual_accel)
 
 
+def apply_carnival_steering_override(car_fingerprint, steering_pressed: bool,
+                                     apply_steer_req: bool, apply_torque: int) -> tuple[bool, int]:
+  if car_fingerprint == CAR.KIA_CARNIVAL_2025 and steering_pressed:
+    return False, 0
+  return apply_steer_req, apply_torque
+
+
 def update_ev9_longitudinal_tuning(state: EV9LongitudinalTuningState, enabled: bool,
                                    stopping: bool, v_ego: float) -> EV9LongitudinalTuningState:
   if not enabled:
@@ -613,6 +620,10 @@ class CarController(CarControllerBase):
       if not CC.latActive:
         apply_torque = 0
 
+      apply_steer_req, apply_torque = apply_carnival_steering_override(
+        self.CP.carFingerprint, CS.out.steeringPressed, apply_steer_req, apply_torque,
+      )
+
       # Hold torque with induced temporary fault when cutting the actuation bit
       # FIXME: we don't use this with CAN FD?
       torque_fault = CC.latActive and not apply_steer_req

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

@@ -20,6 +20,7 @@ from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalT
                                              should_track_stop_accel_directly_for_car, \
                                              preserve_stock_canfd_lfa_status, \
                                              preserve_stock_canfd_lkas_status, \
+                                             apply_carnival_steering_override, \
                                              suppress_redundant_gv70_brake_cancel
 from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state, \
                                              get_canfd_cruise_available
@@ -208,6 +209,12 @@ class TestHyundaiFingerprint:
     assert parser.vl["LKAS_ALT"]["TORQUE_REQUEST"] == 123
     assert parser.vl["LKAS_ALT"]["STEER_REQ"] == 1
 
+  def test_carnival_steering_override_is_scoped_to_2025_platform(self):
+    assert apply_carnival_steering_override(CAR.KIA_CARNIVAL_2025, True, True, 123) == (False, 0)
+    assert apply_carnival_steering_override(CAR.KIA_CARNIVAL_2025, False, True, 123) == (True, 123)
+    assert apply_carnival_steering_override(CAR.KIA_CARNIVAL_HEV_4TH_GEN, True, True, 123) == (True, 123)
+    assert apply_carnival_steering_override(CAR.HYUNDAI_IONIQ_6, True, True, 123) == (True, 123)
+
   def test_canfd_torque_bsm_parser_registers_rear_blindspots(self):
     CP = CarParams.new_message()
     CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN

opendbc_repo/opendbc/car/subaru/carcontroller.py

@@ -30,6 +30,10 @@ _ANGLE_RECLAIM_FRAMES = 36
 _ANGLE_RECLAIM_EXPONENT = 2.5
 _ANGLE_MADS_MIN_SPEED = 0.44704
 _ANGLE_MADS_MAX_STEER_ANGLE = 120.0
+_STOP_START_STARTUP_DELAY_FRAMES = 100
+_STOP_START_STARTUP_DEADLINE_FRAMES = 300
+_STOP_START_PULSE_FRAMES = 30
+_STOP_START_PULSE_PERIOD_FRAMES = 5
 
 
 def get_safety_CP():
@@ -73,6 +77,66 @@ class CarController(CarControllerBase):
     self.prev_close_distance = 0
     self.epb_resume_frames_remaining = -1
     self.last_standstill_frame = 0
+    self.stop_start_attempted = False
+    self.stop_start_request_started = False
+    self.stop_start_request_frame = 0
+    self.stop_start_initial_state = None
+    self.stop_start_counter = 0
+    self.stop_start_acknowledged = False
+
+  def _stop_start_off_request(self, CC, CS, starpilot_toggles):
+    """Send one bounded Outback Stop/Start OFF request after ignition.
+
+    This is intentionally opt-in and limited to a stationary vehicle in
+    Park/Neutral. A single ignition session gets at most one attempt.
+    """
+    if self.CP.carFingerprint != CAR.SUBARU_OUTBACK_2023 or \
+       not getattr(starpilot_toggles, "subaru_stop_start_off", False) or self.stop_start_attempted:
+      return None
+
+    if self.frame > _STOP_START_STARTUP_DEADLINE_FRAMES or getattr(CC, "enabled", False):
+      self.stop_start_attempted = True
+      return None
+
+    if self.frame < _STOP_START_STARTUP_DELAY_FRAMES or not getattr(getattr(CS, "out", None), "canValid", True):
+      return None
+
+    out = CS.out
+    if not getattr(out, "standstill", False) or out.gearShifter not in (
+      structs.CarState.GearShifter.park,
+      structs.CarState.GearShifter.neutral,
+    ):
+      return None
+
+    dashlights_msg = getattr(CS, "dashlights_msg", None)
+    if not dashlights_msg:
+      return None
+
+    if not self.stop_start_request_started:
+      self.stop_start_request_started = True
+      self.stop_start_request_frame = self.frame
+      self.stop_start_initial_state = getattr(CS, "stop_start_state", None)
+      self.stop_start_counter = (int(dashlights_msg.get("COUNTER", 0)) + 1) % 0x10
+
+    current_state = getattr(CS, "stop_start_state", None)
+    if self.stop_start_initial_state is not None and self.stop_start_initial_state != 3 and current_state == 3:
+      self.stop_start_attempted = True
+      self.stop_start_acknowledged = True
+      return None
+
+    elapsed = self.frame - self.stop_start_request_frame
+    if elapsed >= _STOP_START_PULSE_FRAMES:
+      self.stop_start_attempted = True
+      return None
+
+    if elapsed % _STOP_START_PULSE_PERIOD_FRAMES != 0:
+      return None
+
+    msg = subarucan.create_stop_start_control(
+      self.packer, dashlights_msg, counter=self.stop_start_counter, bus=self.main_bus,
+    )
+    self.stop_start_counter = (self.stop_start_counter + 1) % 0x10
+    return msg
 
   def _reset_legacy_2025_handoff(self):
     self.legacy_2025_handoff_active = False
@@ -87,7 +151,7 @@ class CarController(CarControllerBase):
       self._reset_legacy_2025_handoff()
       return False
 
-    if CS.out.steeringPressed:
+    if getattr(CS.out, "steeringPressed", False):
       self.legacy_2025_handoff_active = True
       self.legacy_2025_override_hold_frames = _LEGACY_2025_OVERRIDE_HOLD_FRAMES
       self.legacy_2025_reengage_settle_frames = 0
@@ -96,7 +160,7 @@ class CarController(CarControllerBase):
       return True
 
     if not self.legacy_2025_handoff_active and not self.legacy_2025_lkas_active and \
-       abs(CS.out.steeringRateDeg) > _LEGACY_2025_REENGAGE_MAX_STEER_RATE:
+       abs(getattr(CS.out, "steeringRateDeg", 0.0)) > _LEGACY_2025_REENGAGE_MAX_STEER_RATE:
       self.legacy_2025_handoff_active = True
       self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
 
@@ -109,7 +173,7 @@ class CarController(CarControllerBase):
         self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
       return True
 
-    wheel_stable = abs(CS.out.steeringRateDeg) <= _LEGACY_2025_REENGAGE_MAX_STEER_RATE and \
+    wheel_stable = abs(getattr(CS.out, "steeringRateDeg", 0.0)) <= _LEGACY_2025_REENGAGE_MAX_STEER_RATE and \
       abs(CS.out.steeringAngleDeg - self.legacy_2025_reengage_reference_angle) <= _LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA
     if wheel_stable:
       self.legacy_2025_reengage_settle_frames += 1
@@ -150,7 +214,7 @@ class CarController(CarControllerBase):
       self._reset_angle_handoff()
       return False
 
-    if CS.out.steeringPressed:
+    if getattr(CS.out, "steeringPressed", False):
       self.angle_handoff_active = True
       self.angle_override_hold_frames = _ANGLE_OVERRIDE_HOLD_FRAMES
       self.angle_reengage_settle_frames = 0
@@ -159,7 +223,7 @@ class CarController(CarControllerBase):
       return True
 
     if not self.angle_handoff_active and not self.angle_lkas_active and \
-       abs(CS.out.steeringRateDeg) > _ANGLE_REENGAGE_MAX_STEER_RATE:
+       abs(getattr(CS.out, "steeringRateDeg", 0.0)) > _ANGLE_REENGAGE_MAX_STEER_RATE:
       self.angle_handoff_active = True
       self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
 
@@ -172,7 +236,7 @@ class CarController(CarControllerBase):
         self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
       return True
 
-    wheel_stable = abs(CS.out.steeringRateDeg) <= _ANGLE_REENGAGE_MAX_STEER_RATE and \
+    wheel_stable = abs(getattr(CS.out, "steeringRateDeg", 0.0)) <= _ANGLE_REENGAGE_MAX_STEER_RATE and \
       abs(CS.out.steeringAngleDeg - self.angle_reengage_reference_angle) <= _ANGLE_REENGAGE_MAX_ANGLE_DELTA
     if wheel_stable:
       self.angle_reengage_settle_frames += 1
@@ -267,9 +331,19 @@ class CarController(CarControllerBase):
     elif abs_torque < self.p.STEER_OVERRIDE_TORQUE_LOW:
       self.driver_override = False
 
-    lat_active = CC.latActive and not self.driver_override
+    mads_only = CC.latActive and not getattr(CC, "enabled", False)
+    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 \
+      getattr(CS.out, "gearShifter", structs.CarState.GearShifter.drive) == structs.CarState.GearShifter.drive and \
+      not getattr(CS.out, "standstill", False)
+    manual_handoff = self._angle_manual_handoff(CS, lkas_available)
+    lat_active = lkas_available and not self.driver_override and not manual_handoff
+    if lat_active and not self.angle_lkas_active:
+      self.apply_steer_last = CS.out.steeringAngleDeg
+    steer_target = self._angle_reclaim_target(CC.actuators.steeringAngleDeg) if lat_active else CC.actuators.steeringAngleDeg
     apply_steer = apply_steer_angle_limits_vm(
-      CC.actuators.steeringAngleDeg,
+      steer_target,
       self.apply_steer_last,
       CS.out.vEgoRaw,
       CS.out.steeringAngleDeg,
@@ -282,6 +356,7 @@ class CarController(CarControllerBase):
       apply_steer = CS.out.steeringAngleDeg
 
     self.apply_steer_last = apply_steer
+    self.angle_lkas_active = lat_active
     return subarucan.create_steering_control_angle(self.packer, apply_steer, lat_active, self.angle_bus)
 
   def lateral_torque(self, CC, CS):
@@ -316,6 +391,10 @@ class CarController(CarControllerBase):
 
     can_sends = []
 
+    stop_start_msg = self._stop_start_off_request(CC, CS, starpilot_toggles)
+    if stop_start_msg is not None:
+      can_sends.append(stop_start_msg)
+
     # *** steering ***
     if (self.frame % self.p.STEER_STEP) == 0:
       if self.CP.flags & SubaruFlags.LKAS_ANGLE:

opendbc_repo/opendbc/car/subaru/carstate.py

@@ -4,7 +4,7 @@ from opendbc.can import CANDefine, CANParser
 from opendbc.car import Bus, structs
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.interfaces import CarStateBase
-from opendbc.car.subaru.values import DBC, CanBus, SubaruFlags
+from opendbc.car.subaru.values import CAR, DBC, CanBus, SubaruFlags
 from opendbc.car import CanSignalRateCalculator
 
 
@@ -15,6 +15,8 @@ class CarState(CarStateBase):
     self.shifter_values = can_define.dv["Transmission"]["Gear"]
 
     self.angle_rate_calulator = CanSignalRateCalculator(50)
+    self.dashlights_msg = {}
+    self.stop_start_state = 0
 
   def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
     cp = can_parsers[Bus.pt]
@@ -24,6 +26,10 @@ class CarState(CarStateBase):
     cp_angle = cp_main if self.CP.flags & SubaruFlags.D_PLATFORM else cp
     ret = structs.CarState()
 
+    if self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023:
+      self.dashlights_msg = copy.copy(cp.vl["Dashlights"])
+      self.stop_start_state = cp.vl["Engine_Stop_Start"]["STOP_START_STATE"]
+
     throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_alt.vl["Throttle_Hybrid"]
     ret.gasPressed = throttle_msg["Throttle_Pedal"] > 1e-5
     if self.CP.flags & SubaruFlags.PREGLOBAL:

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_OUTBACK_2023:
+        ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.STOP_START_BUTTON.value
       if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023):
         ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value
 

opendbc_repo/opendbc/car/subaru/subarucan.py

@@ -182,6 +182,20 @@ def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, l
   return packer.make_can_msg("ES_DashStatus", bus, values)
 
 
+def create_stop_start_control(packer, dashlights_msg, counter=None, bus=CanBus.alt):
+  """Create the Outback 2023-24 momentary Stop/Start button request.
+
+  Dashlights is a stock periodic message, so preserve the live frame and only
+  change the event bit. CANPacker calculates the Subaru checksum for us.
+  """
+  values = dict(dashlights_msg)
+  if counter is None:
+    counter = (int(values.get("COUNTER", 0)) + 1) % 0x10
+  values["COUNTER"] = counter % 0x10
+  values["STOP_START"] = 1
+  return packer.make_can_msg("Dashlights", bus, values)
+
+
 def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brake_value, bus=CanBus.main):
   values = {s: es_brake_msg[s] for s in [
     "CHECKSUM",

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

@@ -193,6 +193,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 CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.STOP_START_BUTTON
   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
@@ -205,6 +206,52 @@ def test_outback_2023_uses_d_platform_bus_layout():
   assert CP.lateralSmoothSeconds == pytest.approx(0.4)
 
 
+def test_stop_start_request_is_bounded_and_uses_live_dashlights():
+  CP = CarInterface.get_non_essential_params(CAR.SUBARU_OUTBACK_2023)
+  controller = CarController({}, CP)
+  controller.frame = 101
+
+  class TestActuators:
+    steeringAngleDeg = 0.0
+
+    def as_builder(self):
+      return SimpleNamespace(steeringAngleDeg=self.steeringAngleDeg)
+
+  CC = SimpleNamespace(
+    enabled=False,
+    latActive=False,
+    longActive=False,
+    actuators=TestActuators(),
+    hudControl=SimpleNamespace(leadVisible=False),
+    cruiseControl=SimpleNamespace(cancel=False),
+  )
+  CS = SimpleNamespace(
+    canValid=True,
+    dashlights_msg={"COUNTER": 6, "STOP_START": 0},
+    stop_start_state=0,
+    out=SimpleNamespace(
+      standstill=True,
+      gearShifter=structs.CarState.GearShifter.park,
+    ),
+  )
+  toggles = SimpleNamespace(subaru_stop_start_off=True, subaru_sng=False)
+
+  _, can_sends = controller.update(CC, CS, 0, toggles)
+  stop_start_msgs = [msg for msg in can_sends if msg[0] == 0x390]
+  assert len(stop_start_msgs) == 1
+  assert stop_start_msgs[0][2] == CanBus.alt
+  parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("Dashlights", 0)], CanBus.alt)
+  parser.update([(1, [stop_start_msgs[0]])])
+  assert parser.vl["Dashlights"]["STOP_START"] == 1
+  assert parser.vl["Dashlights"]["COUNTER"] == 7
+
+  controller.frame = 103
+  CS.stop_start_state = 3
+  _, can_sends = controller.update(CC, CS, 0, toggles)
+  assert not any(msg[0] == 0x390 for msg in can_sends)
+  assert controller.stop_start_acknowledged
+
+
 def test_legacy_2025_uses_gen2_angle_bus_layout():
   CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
   parsers = CarState.get_can_parsers(CP)
@@ -448,6 +495,44 @@ def test_angle_controller_tracks_driver_override():
   assert not controller.driver_override
 
 
+def test_angle_controller_blocks_low_speed_mads_engagement():
+  CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
+  controller = CarController({}, CP)
+  CC = SimpleNamespace(
+    enabled=False,
+    latActive=True,
+    actuators=SimpleNamespace(steeringAngleDeg=15.0),
+  )
+  CS = SimpleNamespace(out=SimpleNamespace(
+    vEgoRaw=0.3,
+    steeringAngleDeg=80.0,
+    steeringTorque=0.0,
+    gearShifter=structs.CarState.GearShifter.drive,
+    standstill=False,
+  ))
+  parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
+
+  msg = controller.lateral_angle(CC, CS)
+  parser.update([(1, [msg])])
+
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
+
+  CS.out.vEgoRaw = 1.0
+  CS.out.steeringAngleDeg = 130.0
+  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)
+
+  CS.out.steeringAngleDeg = 0.0
+  msg = controller.lateral_angle(CC, CS)
+  parser.update([(3, [msg])])
+
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
+
+
 def test_ascent_angle_controller_uses_fixed_angle_rate_limits():
   CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
   controller = CarController({}, CP)

opendbc_repo/opendbc/car/subaru/values.py

@@ -88,6 +88,7 @@ class SubaruSafetyFlags(IntFlag):
   D_PLATFORM = 32
   D_PLATFORM_CAMERA = 64
   FIXED_ANGLE_LIMITS = 128
+  STOP_START_BUTTON = 256
   LEGACY_2025_ANGLE_LIMITS = FIXED_ANGLE_LIMITS
 
 

opendbc_repo/opendbc/car/toyota/carcontroller.py

@@ -42,6 +42,7 @@ TOYOTA_NO_LEAD_CRUISE_SIGN_FLIP_MIN_SET_SPEED_ERROR = 0.35  # m/s
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 18  # tx control frames needed before torque can be cut
+TOYOTA_HIGHLANDER_TSS2_MAX_STEER_RATE_FRAMES = 8
 
 # EPS allows user torque above threshold for 50 frames before permanently faulting
 MAX_USER_TORQUE = 500
@@ -72,6 +73,11 @@ def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
   ) or highlander_sdsu)
 
 
+def get_steer_rate_limit_frames(car_fingerprint) -> int:
+  return (TOYOTA_HIGHLANDER_TSS2_MAX_STEER_RATE_FRAMES
+          if car_fingerprint == CAR.TOYOTA_HIGHLANDER_TSS2 else MAX_STEER_RATE_FRAMES)
+
+
 def get_long_tune(CP, params):
   kiBP = [2., 5.]
   kiV = [0.5, 0.25]
@@ -223,6 +229,7 @@ class CarController(CarControllerBase):
     self.standstill_req = False
     self.permit_braking = True
     self.steer_rate_counter = 0
+    self.steer_rate_limit_frames = get_steer_rate_limit_frames(self.CP.carFingerprint)
     self.distance_button = 0
 
     # *** start long control state ***
@@ -345,8 +352,10 @@ class CarController(CarControllerBase):
     apply_torque = apply_meas_steer_torque_limits(new_torque, self.last_torque, CS.out.steeringTorqueEps, self.params)
 
     # >100 degree/sec steering fault prevention
-    self.steer_rate_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE, lat_active,
-                                                                      self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
+    self.steer_rate_counter, apply_steer_req = common_fault_avoidance(
+      abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE, lat_active,
+      self.steer_rate_counter, self.steer_rate_limit_frames,
+    )
 
     if not lat_active:
       apply_torque = 0

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

@@ -10,6 +10,7 @@ from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
 from opendbc.car.toyota import toyotacan
 from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \
                                              get_prius_positive_feedforward_scale, \
+                                             get_steer_rate_limit_frames, \
                                              limit_interceptor_pcm_accel, \
                                              limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \
                                              limit_prius_stopping_accel, should_bypass_toyota_long_pid, update_permit_braking
@@ -706,6 +707,10 @@ class TestToyotaFingerprint:
 
 
 class TestToyotaCarController:
+  def test_highlander_tss2_uses_early_steer_rate_fault_guard(self):
+    assert get_steer_rate_limit_frames(CAR.TOYOTA_HIGHLANDER_TSS2) == 8
+    assert get_steer_rate_limit_frames(CAR.TOYOTA_RAV4_TSS2) == 18
+
   @staticmethod
   def _make_controller(*, standstill_req=False, last_standstill=False):
     controller = CarController.__new__(CarController)

opendbc_repo/opendbc/safety/modes/subaru.h

@@ -41,6 +41,7 @@
 #define MSG_SUBARU_ES_HighBeamAssist     0x121U
 #define MSG_SUBARU_ES_STATIC_1           0x22aU
 #define MSG_SUBARU_ES_STATIC_2           0x325U
+#define MSG_SUBARU_Dashlights            0x390U
 
 #define SUBARU_MAIN_BUS 0U
 #define SUBARU_ALT_BUS  1U
@@ -61,6 +62,9 @@
   {MSG_SUBARU_ES_LKAS_State,   bus, 8, .check_relay = true}, \
   {MSG_SUBARU_ES_Infotainment, bus, 8, .check_relay = true}, \
 
+#define SUBARU_STOP_START_TX_MSGS(bus) \
+  {MSG_SUBARU_Dashlights, bus, 8, .check_relay = false}, \
+
 #define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \
   {MSG_SUBARU_ES_Distance,       alt_bus,         8, .check_relay = true}, \
   {MSG_SUBARU_ES_Brake,          alt_bus,         8, .check_relay = true}, \
@@ -108,6 +112,7 @@ static bool subaru_stop_and_go = false;
 static bool subaru_lkas_angle = false;
 static bool subaru_d_platform = false;
 static bool subaru_fixed_angle_limits = false;
+static bool subaru_stop_start_button = false;
 
 static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
   return (uint8_t)msg->data[0];
@@ -285,6 +290,13 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
     violation |= !(is_tester_present || is_button_rdbi);
   }
 
+  if (msg->addr == MSG_SUBARU_Dashlights) {
+    violation |= !subaru_stop_start_button;
+    violation |= msg->bus != SUBARU_ALT_BUS;
+    violation |= !GET_BIT(msg, 54U);
+    violation |= subaru_get_checksum(msg) != subaru_compute_checksum(msg);
+  }
+
   if (violation){
     tx = false;
   }
@@ -334,6 +346,12 @@ static safety_config subaru_init(uint16_t param) {
     SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
   };
 
+  static const CanMsg SUBARU_D_PLATFORM_ANGLE_STOP_START_MAIN_TX_MSGS[] = {
+    SUBARU_D_PLATFORM_ANGLE_TX_MSGS(SUBARU_MAIN_BUS)
+    SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
+    SUBARU_STOP_START_TX_MSGS(SUBARU_ALT_BUS)
+  };
+
   static const CanMsg SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS[] = {
     SUBARU_D_PLATFORM_ANGLE_TX_MSGS(SUBARU_CAM_BUS)
     SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
@@ -378,6 +396,9 @@ static safety_config subaru_init(uint16_t param) {
   const uint16_t SUBARU_PARAM_FIXED_ANGLE_LIMITS = 128;
   subaru_fixed_angle_limits = GET_FLAG(param, SUBARU_PARAM_FIXED_ANGLE_LIMITS);
 
+  const uint16_t SUBARU_PARAM_STOP_START_BUTTON = 256;
+  subaru_stop_start_button = GET_FLAG(param, SUBARU_PARAM_STOP_START_BUTTON);
+
 #ifdef ALLOW_DEBUG
   const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
   subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
@@ -385,8 +406,9 @@ static safety_config subaru_init(uint16_t param) {
 
   safety_config ret;
   if (subaru_lkas_angle) {
-    ret = subaru_d_platform ? (subaru_d_platform_camera ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS) : \
-                              BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_MAIN_TX_MSGS)) : \
+    ret = subaru_d_platform ? (subaru_stop_start_button ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_STOP_START_MAIN_TX_MSGS) : \
+                              (subaru_d_platform_camera ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS) : \
+                              BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_MAIN_TX_MSGS))) : \
           subaru_gen2 ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_TX_MSGS) : \
                         BUILD_SAFETY_CFG(subaru_lkas_angle_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS);
   } else if (subaru_gen2) {

opendbc_repo/opendbc/safety/tests/common.py

@@ -1075,13 +1075,14 @@ class SafetyTest(SafetyTestBase):
               continue
             if attr.startswith('TestSubaruGen') and current_test.startswith('TestSubaruGen'):
               continue
-            if 'TestSubaruDPlatformAngleSafety' in {attr, current_test} and \
-                'Angle' in attr and 'Angle' in current_test:
+            if attr.startswith('TestSubaruDPlatform') and current_test.startswith('TestSubaruDPlatform'):
               continue
-            if 'TestSubaruDPlatformAngleSafety' in {attr, current_test}:
+            if attr.startswith('TestSubaruDPlatform'):
               # D-platform uses the same main-bus HUD messages as the other
               # Subaru modes, so those modes cannot be distinguished by ID.
-              tx = list(filter(lambda m: not (m[1] == 0 and m[0] in [0x321, 0x322, 0x323]), tx))
+              tx = list(filter(lambda m: not (m[1] == 0 and m[0] in [0x124, 0x321, 0x322, 0x323]), tx))
+            if current_test.startswith('TestSubaruDPlatform') and attr.startswith('TestSubaruGen'):
+              tx = list(filter(lambda m: not (m[1] == 0 and m[0] in [0x124, 0x321, 0x322, 0x323]), tx))
             if attr.startswith('TestSubaruPreglobal') and current_test.startswith('TestSubaruPreglobal'):
               continue
             if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}):

opendbc_repo/opendbc/safety/tests/test_subaru.py

@@ -36,6 +36,7 @@ class SubaruMsg(enum.IntEnum):
   ES_HighBeamAssist = 0x121
   ES_STATIC_1       = 0x22a
   ES_STATIC_2       = 0x325
+  Dashlights        = 0x390
 
 
 SUBARU_MAIN_BUS = 0
@@ -401,6 +402,21 @@ class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, Test
     return self.packer.make_can_msg_safety("Steering_2", SUBARU_MAIN_BUS, {"Steering_Angle": angle})
 
 
+class TestSubaruDPlatformStopStartSafety(TestSubaruDPlatformAngleSafety):
+  FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM | \
+    SubaruSafetyFlags.STOP_START_BUTTON
+  TX_MSGS = TestSubaruDPlatformAngleSafety.TX_MSGS + [[SubaruMsg.Dashlights, SUBARU_ALT_BUS]]
+
+  def _stop_start_msg(self, pressed):
+    return self.packer.make_can_msg_safety(
+      "Dashlights", SUBARU_ALT_BUS, {"COUNTER": 0, "STOP_START": pressed},
+    )
+
+  def test_stop_start_tx_requires_pressed_bit(self):
+    self.assertTrue(self._tx(self._stop_start_msg(True)))
+    self.assertFalse(self._tx(self._stop_start_msg(False)))
+
+
 class TestSubaruDPlatformCameraAngleSafety(TestSubaruDPlatformAngleSafety):
   FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM | SubaruSafetyFlags.D_PLATFORM_CAMERA
   TX_MSGS = [[SubaruMsg.ES_LKAS_ANGLE, SUBARU_CAM_BUS],

panda/board/obj/gitversion.h

@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-f8d47139-DEBUG";
+const uint8_t gitversion[19] = "DEV-3c655e79-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-f8d47139-DEBUG
+DEV-3c655e79-DEBUG

scripts/host_tool_runner.sh

@@ -345,6 +345,8 @@ sync_worktree() {
     "msgq_repo/msgq/ipc_pyx.so"
     "msgq_repo/msgq/visionipc/visionipc_pyx.so"
     "rednose_repo/rednose/helpers/ekf_sym_pyx.so"
+    "selfdrive/locationd/models/generated/*.so"
+    "selfdrive/locationd/models/generated/*.os"
     "selfdrive/modeld/models/commonmodel_pyx.so"
     "selfdrive/pandad/libcan_list_to_can_capnp.a"
     "selfdrive/pandad/pandad_api_impl.so"

scripts/model_compiler.py

@@ -16,6 +16,8 @@ REPO_ROOT = Path(__file__).resolve().parents[1]
 if str(REPO_ROOT) not in sys.path:
   sys.path.insert(0, str(REPO_ROOT))
 
+from openpilot.starpilot.common.model_versions import UNIFIED_ARTIFACT_FORMAT
+
 DEFAULT_INPUT_ROOT = Path("/data/openpilot/uncompiledmodels")
 DEFAULT_OUTPUT_ROOT = Path("/data/openpilot/compiledmodels")
 COMPILE_SCRIPT = REPO_ROOT / "tinygrad_repo/examples/openpilot/compile3.py"
@@ -389,15 +391,33 @@ def multipart_output_paths(artifact: Path, output_dir: Path | None = None) -> li
   ]
 
 
-def install_local_artifact(artifact: Path, model_key: str, version: str) -> None:
+def _update_local_artifact_metadata(model_key: str, external_gpu: bool) -> None:
+  metadata_path = MODELS_PATH / ".model_artifacts.json"
+  try:
+    payload = json.loads(metadata_path.read_text()) if metadata_path.is_file() else {}
+    if not isinstance(payload, dict):
+      payload = {}
+    metadata = payload.get(model_key)
+    if not isinstance(metadata, dict):
+      metadata = {}
+    metadata["artifact_format"] = UNIFIED_ARTIFACT_FORMAT
+    metadata["uses_external_gpu"] = bool(external_gpu)
+    payload[model_key] = metadata
+    metadata_path.write_text(json.dumps(payload))
+  except Exception as error:
+    print(f"  WARN: could not update {metadata_path.name}: {error}")
+
+
+def install_local_artifact(artifact: Path, model_key: str, version: str, external_gpu: bool = False) -> None:
   """Copy a freshly compiled local- model into the runtime dir modeld loads from,
-  and ensure its <id>.json sidecar carries the correct version.
+  and ensure its <id>.json sidecar and artifact cache carry the correct metadata.
 
   The sidecar version is NOT cosmetic: without it _discover_local_models() records
   an empty version, which downstream parses on the wrong contract (a v15 model then
   drives like v11). Since we know the build version here, we write it so the local
-  install is correct by default. Local models must be a single is_file() in
-  /data/models to show in the picker. No-ops off-device (no /data/models).
+  install is correct by default. The GPU flag is equally important: it selects
+  the out-of-band loader and AMD queue at runtime. Local models must be a single
+  is_file() in /data/models to show in the picker. No-ops off-device.
   """
   if not MODELS_PATH.is_dir():
     print(f"  skipped auto-install: {MODELS_PATH} not present (not on device?)")
@@ -415,20 +435,19 @@ def install_local_artifact(artifact: Path, model_key: str, version: str) -> None
         info = loaded
     except Exception as error:
       print(f"  WARN: existing sidecar {sidecar.name} is malformed, rewriting: {error}")
-  if not version:
-    if not str(info.get("version") or "").strip():
-      print(f"  WARN: could not determine version -- set it by hand in {sidecar.name} "
-            "or the model may drive on the wrong version contract")
-    return
-  # keep any user-set name/series; only guarantee a correct, non-empty version
-  if str(info.get("version") or "").strip() == version and sidecar.is_file():
-    print(f"  sidecar ok: {sidecar.name} (version {version})")
-    return
-  info.setdefault("name", model_key[len("local-"):].replace("_", " ").replace("-", " ").strip())
-  info.setdefault("series", "Local")
-  info["version"] = version
-  sidecar.write_text(json.dumps(info, indent=2) + "\n")
-  print(f"  wrote sidecar {sidecar.name} (version {version})")
+  if version:
+    info.setdefault("name", model_key[len("local-"):].replace("_", " ").replace("-", " ").strip())
+    info.setdefault("series", "Local")
+    info["version"] = version
+  elif not str(info.get("version") or "").strip():
+    print(f"  WARN: could not determine version -- set it by hand in {sidecar.name} "
+          "or the model may drive on the wrong version contract")
+
+  info["uses_external_gpu"] = bool(external_gpu)
+  if version or sidecar.is_file() or external_gpu:
+    sidecar.write_text(json.dumps(info, indent=2) + "\n")
+    print(f"  wrote sidecar {sidecar.name} (gpu={bool(external_gpu)})")
+  _update_local_artifact_metadata(model_key, external_gpu)
 
 
 def split_oversized_artifact(
@@ -534,7 +553,7 @@ def compile_driving(
     for qcom_only_flag in ("IMAGE", "NOLOCALS", "OPENPILOT_HACKS"):
       compile_env.pop(qcom_only_flag, None)
     compile_env.update({
-      "DEBUG": "2",
+      "DEBUG": "1",
       "DEV": "USB+AMD:LLVM",
       "WARP_DEV": "QCOM",
       "FLOAT16": "1",
@@ -665,7 +684,7 @@ def main() -> int:
         print(f"  --no-split: kept one {size_mb:.1f} MB file; over 100 MB, so split it "
               "before committing to a repo (re-run without --no-split, or --split-artifact)")
     if is_local and not args.no_install:
-      install_local_artifact(output, model_key, version)
+      install_local_artifact(output, model_key, version, args.external_gpu)
   else:
     multipart_outputs = split_oversized_artifact(output)
     if multipart_outputs:

selfdrive/car/card.py

@@ -85,7 +85,8 @@ class Car:
   def __init__(self, CI=None, RI=None) -> None:
     self.can_sock = messaging.sub_sock('can', timeout=20)
     self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents', 'radarState', 'longitudinalPlan'])
-    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks', 'gpsLocationExternal'])
+    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'])
+    self.gps_pm = None
 
     self.can_rcv_cum_timeout_counter = 0
     self._last_car_gps_timestamp_nanos = 0
@@ -138,7 +139,10 @@ class Car:
       self.CI, self.CP, self.FPCP = CI, CI.CP, CI.FPCP
       self.RI = RI
 
-    self.params.put_bool("CarGpsAvailable", bool(getattr(self.CI.CS, 'car_gps_supported', False)))
+    car_gps_supported = bool(getattr(self.CI.CS, 'car_gps_supported', False))
+    self.params.put_bool("CarGpsAvailable", car_gps_supported)
+    if car_gps_supported:
+      self.gps_pm = messaging.PubMaster(['gpsLocationExternal'])
 
     interface_alternative_experience = self.CP.alternativeExperience
     self.CP.alternativeExperience = interface_alternative_experience
@@ -364,7 +368,8 @@ class Car:
       gps.speedAccuracy = car_gps['speedAccuracy']
       gps.hasFix = car_gps['hasFix']
       gps.satelliteCount = car_gps['satelliteCount']
-      self.pm.send('gpsLocationExternal', gps_send)
+      assert self.gps_pm is not None
+      self.gps_pm.send('gpsLocationExternal', gps_send)
       self._last_car_gps_publish_monotonic = now
 
     # carParams - logged every 50 seconds (> 1 per segment)

selfdrive/controls/lib/lane_centering.py

@@ -14,6 +14,7 @@ _MAX_OFFSET = 0.3
 _MIN_CENTER_TO_LINE = 1.1
 _MAX_RAW_CORRECTION = 0.004
 _MAX_GAIN = 0.30
+_VISUAL_CORRECTION_EPSILON = 1e-6
 _SMOOTH_TAU = 0.4
 _SIGNAL_RELEASE_TAU = 0.20
 _CONFIDENCE_RELEASE_TAU = 0.20
@@ -183,8 +184,9 @@ def get_lane_centering_visual_direction(model_v2, v_ego: float, offset: float, e
   )
   if not valid or not np.isfinite(correction):
     return 0
-  if correction == 0.0:
-    if applied_correction is None or not np.isfinite(applied_correction) or applied_correction == 0.0:
-      return 0
-    correction = applied_correction
+  if applied_correction is not None and np.isfinite(applied_correction) and \
+      abs(applied_correction) > _VISUAL_CORRECTION_EPSILON:
+    correction = float(applied_correction)
+  if abs(correction) <= _VISUAL_CORRECTION_EPSILON:
+    return 0
   return 1 if correction > 0.0 else -1

selfdrive/controls/lib/latcontrol_angle.py

@@ -6,6 +6,8 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 
 # TODO This is speed dependent
 STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
+FORD_ANGLE_CATCH_UP_HORIZON = 1.25  # Seconds
+FORD_ANGLE_RATE_FILTER_TIME_CONSTANT = 0.15  # Seconds
 
 _ASCENT_ANGLE_TRACKING_GAIN = 0.25
 _ASCENT_ANGLE_TRACKING_MAX_CORRECTION = 8.0
@@ -23,12 +25,32 @@ def _ascent_angle_tracking_target(target_angle: float, steering_angle: float,
   return target_angle + correction
 
 
+def _ford_angle_tracking_saturated(angle_error: float, steering_rate: float) -> bool:
+  """Only call a Ford angle request saturated when the EPS is not on track to catch it."""
+  catching_up = angle_error * steering_rate > 0.0
+  catching_up &= abs(angle_error) <= abs(steering_rate) * FORD_ANGLE_CATCH_UP_HORIZON
+  return abs(angle_error) > STEER_ANGLE_SATURATION_THRESHOLD and not catching_up
+
+
 class LatControlAngle(LatControl):
   def __init__(self, CP, CI, dt):
     super().__init__(CP, CI, dt)
     self.sat_check_min_speed = 5.
     self.use_steer_limited_by_safety = CP.brand in ("tesla", "hyundai")
     self.is_ascent = CP.carFingerprint == SUBARU_CAR.SUBARU_ASCENT_2023
+    self.is_ford = CP.brand == "ford"
+    self.measured_angle_last = None
+    self.measured_angle_rate = 0.0
+
+  def _update_measured_angle_rate(self, steering_angle: float, reset: bool) -> float:
+    if reset or self.measured_angle_last is None:
+      self.measured_angle_rate = 0.0
+    else:
+      raw_rate = (steering_angle - self.measured_angle_last) / self.dt
+      alpha = self.dt / (FORD_ANGLE_RATE_FILTER_TIME_CONSTANT + self.dt)
+      self.measured_angle_rate += alpha * (raw_rate - self.measured_angle_rate)
+    self.measured_angle_last = steering_angle
+    return self.measured_angle_rate
 
   def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles):
     angle_log = log.ControlsState.LateralAngleState.new_message()
@@ -49,13 +71,21 @@ class LatControlAngle(LatControl):
           bool(getattr(CS, "steeringPressed", False)),
         )
 
+    ford_angle_mode = self.is_ford and getattr(starpilot_toggles, "ford_lateral_mode", -1) == 2
+    measured_angle_rate = self._update_measured_angle_rate(
+      float(CS.steeringAngleDeg), not active or bool(CS.steeringPressed) or not ford_angle_mode)
+
     if self.use_steer_limited_by_safety:
       # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation
       angle_control_saturated = steer_limited_by_safety
     else:
       # for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota)
       # or relying on EPS (Ford Q3), carOutput does not capture maxing out torque  # TODO: this can be improved
-      angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
+      angle_error = angle_steers_des - CS.steeringAngleDeg
+      if ford_angle_mode:
+        angle_control_saturated = _ford_angle_tracking_saturated(angle_error, measured_angle_rate)
+      else:
+        angle_control_saturated = abs(angle_error) > STEER_ANGLE_SATURATION_THRESHOLD
     angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
     angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     angle_log.steeringAngleDesiredDeg = angle_steers_des

selfdrive/controls/lib/latcontrol_torque.py

@@ -108,6 +108,7 @@ class LatControlTorque(LatControl):
     self.is_genesis_gv70 = CP.carFingerprint in GENESIS_GV70_CARS
     self.is_palisade = CP.carFingerprint in PALISADE_CARS
     self.is_prius = CP.carFingerprint in PRIUS_CARS
+    self.is_standard_prius = CP.carFingerprint == TOYOTA_CAR.TOYOTA_PRIUS
     self.is_camry = CP.carFingerprint in CAMRY_CARS
     self.is_rav4_tss2 = CP.carFingerprint in RAV4_TSS2_CARS
     self.is_rav4_prime = CP.carFingerprint in RAV4_PRIME_CARS
@@ -610,7 +611,9 @@ class LatControlTorque(LatControl):
         output_torque *= get_toyota_corolla_tss2_center_output_scale(setpoint, CS.vEgo)
       elif prius_active:
         output_torque *= prius_center_taper
-        output_torque *= get_prius_high_speed_output_taper_scale(setpoint, CS.vEgo)
+        prius_taper_max = (PRIUS_STANDARD_HIGH_SPEED_OUTPUT_TAPER_MAX if self.is_standard_prius
+                           else PRIUS_HIGH_SPEED_OUTPUT_TAPER_MAX)
+        output_torque *= get_prius_high_speed_output_taper_scale(setpoint, CS.vEgo, prius_taper_max)
       elif volt_standard_test_active:
         output_torque *= volt_standard_center_taper
       elif volt_plexy_test_active:

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -247,7 +247,7 @@ GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30
 GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08
 GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0
 GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5
-GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.14
+GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.16
 GENESIS_G70_CENTER_OUTPUT_TAPER_LAT = 0.30
 GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10
 GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED = 18.0
@@ -277,7 +277,7 @@ GENESIS_G70_CURVE_UNWIND_LAT = 0.25
 GENESIS_G70_CURVE_UNWIND_LAT_WIDTH = 0.12
 GENESIS_G70_CURVE_UNWIND_JERK = 0.08
 GENESIS_G70_CURVE_UNWIND_JERK_WIDTH = 0.08
-GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.28
+GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.32
 GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.12
 GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.12
 GENESIS_G70_UNWIND_FF_JERK = 0.10
@@ -1035,6 +1035,7 @@ PRIUS_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.07
 PRIUS_FRICTION_JERK_DEADZONE_SPEED = 18.0
 PRIUS_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 2.2
 PRIUS_HIGH_SPEED_OUTPUT_TAPER_MAX = 0.06
+PRIUS_STANDARD_HIGH_SPEED_OUTPUT_TAPER_MAX = 0.10
 PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT = 0.30
 PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT_WIDTH = 0.35
 PRIUS_HIGH_SPEED_OUTPUT_TAPER_SPEED = 22.0
@@ -1493,12 +1494,13 @@ def get_prius_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float)
   return PRIUS_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight
 
 
-def get_prius_high_speed_output_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
+def get_prius_high_speed_output_taper_scale(desired_lateral_accel: float, v_ego: float,
+                                            taper_max: float = PRIUS_HIGH_SPEED_OUTPUT_TAPER_MAX) -> float:
   speed_weight = _prius_sigmoid((v_ego - PRIUS_HIGH_SPEED_OUTPUT_TAPER_SPEED) /
                                 PRIUS_HIGH_SPEED_OUTPUT_TAPER_SPEED_WIDTH)
   curve_weight = _prius_sigmoid((abs(desired_lateral_accel) - PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT) /
                                  PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT_WIDTH)
-  return 1.0 - PRIUS_HIGH_SPEED_OUTPUT_TAPER_MAX * speed_weight * curve_weight
+  return 1.0 - taper_max * speed_weight * curve_weight
 
 
 def get_camry_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,

selfdrive/controls/lib/lead_behavior.py

@@ -113,7 +113,9 @@ def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lea
 
 
 def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_gap: float, closing_speed: float,
-                                  v_cruise: float | None = None, y_rel: float | None = None) -> float:
+                                  v_cruise: float | None = None, y_rel: float | None = None,
+                                  min_headway_margin: float = TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN,
+                                  full_headway_margin: float = TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN) -> float:
   gap_error = lead_distance - desired_gap
   actual_hw = lead_distance / max(v_ego, 1e-3)
   desired_hw = desired_gap / max(v_ego, 1e-3)
@@ -137,8 +139,8 @@ def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_ga
   fade_end_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN,
                         TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END - desired_hw)
   entry_factor = _smoothstep(headway_margin,
-                             TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN,
-                             TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN)
+                             min_headway_margin,
+                             full_headway_margin)
   exit_factor = 1.0 - _smoothstep(headway_margin, fade_start_margin, fade_end_margin)
 
   closing_fade_end = max(2.5, 0.12 * v_ego)

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -924,7 +924,7 @@ class LongitudinalMpc:
              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,
-             lead_obstacle_bias=(0.0, 0.0)):
+             lead_obstacle_bias=(0.0, 0.0), tracked_lead_catchup_headway_margins=None):
     v_ego = self.x0[1]
     lead_one = radarstate.leadOne
     lead_two = radarstate.leadTwo
@@ -972,6 +972,12 @@ class LongitudinalMpc:
       if optional_far_lead_comfort and tracking_lead and lead_one.status:
         desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow)
         closing_speed = max(0.0, v_ego - lead_one.vLead)
+        catchup_kwargs = {}
+        if tracked_lead_catchup_headway_margins is not None:
+          catchup_kwargs = {
+            "min_headway_margin": tracked_lead_catchup_headway_margins[0],
+            "full_headway_margin": tracked_lead_catchup_headway_margins[1],
+          }
         cruise_obstacle += get_tracked_lead_catchup_bias(
           v_ego,
           lead_one.dRel,
@@ -979,6 +985,7 @@ class LongitudinalMpc:
           closing_speed,
           v_cruise=v_cruise,
           y_rel=float(getattr(lead_one, "yRel", 0.0)),
+          **catchup_kwargs,
         )
       if optional_far_lead_comfort:
         cruise_obstacle += self.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow)

selfdrive/controls/lib/longitudinal_planner.py

@@ -7,8 +7,10 @@ from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.constants import CV
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.starpilot.common.model_versions import is_tinygrad_model_version
+from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode
 from openpilot.starpilot.controls.lib.starpilot_vcruise import FT_TO_M, OFFSET_FT_MAX, OFFSET_FT_MIN
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
@@ -37,8 +39,11 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   get_honda_crv_5g_stopped_lead_obstacle_bias,
   get_honda_crv_5g_low_speed_stopped_lead_cap,
   allow_honda_crv_5g_vision_gap_settle,
+  get_honda_crv_5g_early_radar_follow_cap,
   get_standstill_gap_settle_max_extra_gap,
   get_standstill_stopped_lead_guard_distance_margin,
+  get_standstill_stopped_lead_guard_max_lead_speed,
+  get_tracked_lead_catchup_headway_margins,
 )
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
@@ -139,6 +144,26 @@ VISION_LEAD_APPROACH_BRAKING_DEFICIT_MIN = 0.75
 VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE = 0.45
 VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE = 1.20
 PLANNER_SAFETY_WARNING_INTERVAL = 5.0
+HEM_STATUS_LOG_INTERVAL = 10.0
+HEM_AUTH_PUB_INTERVAL = 0.5
+
+
+def _hem_log_timestamp() -> str:
+  """Wall-clock timestamp with millisecond precision for the [HEM] live log."""
+  from datetime import datetime
+  now = datetime.now()
+  return now.strftime("%H:%M:%S") + f".{now.microsecond // 1000:03d}"
+
+
+def _hem_format_diag_value(value):
+  """Compact, deterministic formatting for the per-frame HEM diagnostic dump."""
+  if isinstance(value, (bool, np.bool_)):
+    return "1" if value else "0"
+  if isinstance(value, float):
+    return f"{value:.4f}"
+  if isinstance(value, (int, np.integer)):
+    return str(int(value))
+  return str(value)
 VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL = 1.30
 VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL = 1.75
 VISION_LEAD_APPROACH_CONFIRM_TIME = 0.25
@@ -560,6 +585,7 @@ class LongitudinalPlanner:
     self.output_should_stop = False
     self.far_follow_brake_slew_rate, self.far_follow_release_slew_rate = get_far_follow_output_slew_rates(CP)
     self.untracked_slow_lead_decel_scale = get_untracked_slow_lead_decel_scale(CP)
+    self.tracked_lead_catchup_headway_margins = get_tracked_lead_catchup_headway_margins(CP)
     self.far_follow_output_slew_active = False
     self.model_launch_armed = False
     self.model_launch_stop_seen = False
@@ -607,6 +633,11 @@ class LongitudinalPlanner:
     self.duplicate_vision_comfort_lead_source = None
     self.prev_experimental_mode = None
     self.experimental_release_accel_until = 0.0
+    self.hybrid_controller = HybridExperimentalMode()
+    self._hem_status_log_t = 0.0
+    self._hem_logged_active = False
+    self._hem_auth_pub_t = 0.0
+    self._hem_params_memory = None
 
     if self.is_preap:
       try:
@@ -1638,13 +1669,16 @@ class LongitudinalPlanner:
 
     lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0)
     lead_delta = lead_speed - float(v_ego)
+    max_lead_speed = get_standstill_stopped_lead_guard_max_lead_speed(
+      self.CP, STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED,
+    )
     max_distance = max(
       STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE,
       float(stop_distance) + get_standstill_stopped_lead_guard_distance_margin(self.CP),
     )
     if (
       float(getattr(lead, "dRel", float("inf"))) > max_distance or
-      lead_speed > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED or
+      lead_speed > max_lead_speed or
       lead_delta > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA
     ):
       return None
@@ -1652,7 +1686,7 @@ class LongitudinalPlanner:
     distance_factor = float(np.clip((max_distance - float(lead.dRel)) /
                                     max(max_distance - STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE, 0.1),
                                     0.0, 1.0))
-    speed_factor = float(np.clip(lead_speed / max(STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED, 0.1), 0.0, 1.0))
+    speed_factor = float(np.clip(lead_speed / max(max_lead_speed, 0.1), 0.0, 1.0))
     delta_factor = float(np.clip((STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA - lead_delta) /
                                  max(STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA, 0.1),
                                  0.0, 1.0))
@@ -1903,6 +1937,36 @@ class LongitudinalPlanner:
       floor = min(LC_MERGE_ACCEL_BIAS, cruise_cap)
     return floor
 
+  def _log_hem_status(self, now_t, active, v_ego, a_chill, a_exp, a_fused):
+    hc = self.hybrid_controller
+    hc.record_diag = True
+    ts = _hem_log_timestamp()
+    if active != self._hem_logged_active:
+      self._hem_logged_active = active
+      self._hem_status_log_t = 0.0
+      print(f"[HEM] {ts} mode {'ON' if active else 'OFF'}")
+    if not active:
+      return
+    # Rich per-frame dump of every HEM decision variable so a missed stop can be
+    # diagnosed to the exact frame and signal (see hybrid_experimental_mode diag).
+    d = hc.diag
+    if d:
+      print(f"[HEM] {ts} " + " ".join(f"{k}={_hem_format_diag_value(v)}" for k, v in d.items()))
+
+  def _publish_hem_status(self, now_t):
+    if self._hem_params_memory is None:
+      try:
+        self._hem_params_memory = Params(memory=True)
+      except Exception:
+        return
+    if now_t - self._hem_auth_pub_t < HEM_AUTH_PUB_INTERVAL:
+      return
+    self._hem_auth_pub_t = now_t
+    try:
+      self._hem_params_memory.put_bool("HEMExpDominant", bool(self.hybrid_controller.last_exp_dominant))
+    except Exception:
+      pass
+
   def update(self, sm, starpilot_toggles):
     if self.is_preap:
       self._preap_param_frame += 1
@@ -1955,6 +2019,7 @@ class LongitudinalPlanner:
       self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
       self.model_allow_throttle = True
       self.model_allow_throttle_transition_t = 0.0
+      self.hybrid_controller.reset(float(self.a_desired))
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -2261,7 +2326,8 @@ class LongitudinalPlanner:
                     stop_x=force_stop_x,
                     silverado_early_follow=early_truck_follow,
                     modelV2=sm['modelV2'],
-                    lead_obstacle_bias=stopped_lead_obstacle_bias)
+                    lead_obstacle_bias=stopped_lead_obstacle_bias,
+                    tracked_lead_catchup_headway_margins=self.tracked_lead_catchup_headway_margins)
 
     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)
@@ -2315,26 +2381,31 @@ class LongitudinalPlanner:
       model_launch_accel = self.get_model_launch_accel(model_launch_v, model_launch_a, action_t, scene_v_ego)
 
     if classic_model:
-      output_a_target, output_should_stop = get_accel_from_plan_classic(
+      output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan_classic(
         self.CP, self.v_desired_trajectory, self.a_desired_trajectory, starpilot_toggles.vEgoStopping)
     elif tinygrad_model:
       output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(
         self.v_desired_trajectory, self.a_desired_trajectory,
         action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping)
-      output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
-      output_should_stop_e2e = sm['modelV2'].action.shouldStop
-
-      if self.mode == 'acc' or self.generation == 'v9':
-        output_a_target = output_a_target_mpc
-        output_should_stop = output_should_stop_mpc
-      else:
-        output_a_target = min(output_a_target_mpc, output_a_target_e2e)
-        output_should_stop = output_should_stop_e2e or output_should_stop_mpc
     else:
-      output_a_target, output_should_stop = get_accel_from_plan(
+      output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(
         self.v_desired_trajectory, self.a_desired_trajectory,
         action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping)
 
+    if bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)):
+      output_a_target = output_a_target_mpc
+      output_should_stop = output_should_stop_mpc
+    elif tinygrad_model and self.mode != 'acc' and self.generation != 'v9':
+      output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
+      output_should_stop_e2e = sm['modelV2'].action.shouldStop
+      output_a_target = min(output_a_target_mpc, output_a_target_e2e)
+      output_should_stop = output_should_stop_e2e or output_should_stop_mpc
+      self._log_hem_status(now_t, False, scene_v_ego, output_a_target_mpc, output_a_target_e2e, output_a_target)
+    else:
+      output_a_target = output_a_target_mpc
+      output_should_stop = output_should_stop_mpc
+      self._log_hem_status(now_t, False, scene_v_ego, output_a_target_mpc, float('nan'), output_a_target)
+
     comfort_output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0]
     vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego))
     output_accel_min = comfort_output_accel_min
@@ -2351,6 +2422,22 @@ class LongitudinalPlanner:
       if approach_lift_caps:
         raw_approach_lift_cap = min(approach_lift_caps)
 
+      if not experimental_mode:
+        early_radar_caps = [
+          cap for cap in (
+            get_honda_crv_5g_early_radar_follow_cap(
+              self.CP, self.lead_one, v_ego, vision_cap_accel_min,
+            ),
+            get_honda_crv_5g_early_radar_follow_cap(
+              self.CP, self.lead_two, v_ego, vision_cap_accel_min,
+            ),
+          ) if cap is not None
+        ]
+        if early_radar_caps:
+          early_radar_cap = min(early_radar_caps)
+          self.a_desired = min(self.a_desired, early_radar_cap)
+          output_a_target = min(output_a_target, early_radar_cap)
+
       pretracking_vision_caps = []
       for lead in (self.lead_one, self.lead_two):
         if lead.status and not bool(getattr(lead, "radar", False)):
@@ -2951,8 +3038,56 @@ class LongitudinalPlanner:
     if force_slow_decel and scene_v_ego > 0.1:
       output_a_target = min(output_a_target, FORCE_DECEL_MIN_ACCEL)
 
-    self.output_a_target = output_a_target
-    self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active)
+    a_chill_final = output_a_target
+    should_stop_chill = bool(output_should_stop or vision_low_speed_stop_active)
+
+    if bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)):
+      self.hybrid_controller.set_tuning(
+        getattr(starpilot_toggles, "hybrid_exp_bias", 0.0),
+        getattr(starpilot_toggles, "hybrid_vision_brake_sensitivity", 1.0),
+      )
+      a_exp_raw = float(sm['modelV2'].action.desiredAcceleration)
+      should_stop_exp = bool(sm['modelV2'].action.shouldStop)
+
+      # Pass the active lead that MPC is tracking (leadTwo when source == "lead1")
+      # so HEM is never blind to the radar lead actually being followed.
+      active_lead = self.lead_two if self.mpc.source == "lead1" else self.lead_one
+
+      a_fused, should_stop_fused = self.hybrid_controller.update(
+        v_ego=scene_v_ego,
+        v_cruise=v_cruise,
+        lead_one=active_lead,
+        model_v2=sm['modelV2'],
+        a_chill=a_chill_final,
+        a_exp=a_exp_raw,
+        should_stop_exp=should_stop_exp,
+        should_stop_chill=should_stop_chill,
+        gas_pressed=bool(getattr(sm['carState'], 'gasPressed', False)),
+      )
+
+      # HEM output can never exceed the physical
+      # vehicle acceleration envelope (same bounds as the non-hybrid path) or a
+      # per-frame jerk slew from the previously commanded target, regardless of
+      # tuning bias.
+      a_fused = float(np.clip(a_fused, output_accel_min, output_accel_max))
+      if not np.isfinite(a_fused):
+        a_fused = float(a_chill_final)
+
+      max_jerk_accel = float(getattr(sm['starpilotPlan'], 'accelerationJerk', 1.0)) * 3.0
+      max_jerk_brake = 4.0
+      max_delta_up = max_jerk_accel * self.dt
+      max_delta_down = max_jerk_brake * self.dt
+      prev_target = float(prev_output_a_target)
+      a_fused = float(np.clip(a_fused, prev_target - max_delta_down, prev_target + max_delta_up))
+
+      self.output_a_target = a_fused
+      self.output_should_stop = should_stop_fused
+      self._log_hem_status(now_t, True, scene_v_ego, a_chill_final, a_exp_raw, a_fused)
+      self._publish_hem_status(now_t)
+
+    else:
+      self.output_a_target = a_chill_final
+      self.output_should_stop = should_stop_chill
 
   def publish(self, sm, pm):
     plan_send = messaging.new_message('longitudinalPlan')