Flood Warning

Merge pull request #194 from N30-PH/starpilot-honda-city-2025-firmware
Honda: add observed 2025 City firmware versions
2026-10-02 04:13:46 +08:00 · 2026-10-01 14:11:28 -05:00 · 2026-10-01 12:43:20 -05:00 · 2026-09-30 17:16:41 -05:00 · 2026-09-30 16:13:34 -03:00 · 2026-09-30 16:13:21 -03:00
34 changed files with 1086 additions and 104 deletions
@@ -264,7 +264,7 @@ A supported vehicle is one that just works when you install a comma device. All
 |Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|6 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Forte 2019-21">Buy Here</a></sub></details>|||
 |Kia|Forte 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Forte 2022-23">Buy Here</a></sub></details>|||
 |Kia|K4 (with HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai R connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (with HDA II) 2025">Buy Here</a></sub></details>|||
-|Kia|K4 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (without HDA II) 2025">Buy Here</a></sub></details>|||
+|Kia|K4 (without HDA II) 2025-26|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (without HDA II) 2025-26">Buy Here</a></sub></details>|||
 |Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 2021-24">Buy Here</a></sub></details>|||
 |Kia|K5 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai M connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 (without HDA II) 2025">Buy Here</a></sub></details>|||
 |Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 Hybrid 2020-22">Buy Here</a></sub></details>|||
@@ -4,7 +4,7 @@ from opendbc.can import CANPacker
 from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, structs
 from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits
 from opendbc.car.ford import fordcan
-from opendbc.car.ford.values import CarControllerParams, FordFlags
+from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags
 from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
 # This Ford extension boundary substantially adapts BluePilot bp-7.0 work. See the root CREDITS.md
 # (including Alan Polk's d0aac605f and db2bdff05) and THIRD_PARTY_NOTICES.md.
@@ -64,7 +64,9 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c
  return apply_curvature


-def apply_creep_compensation(accel: float, v_ego: float) -> float:
+def apply_creep_compensation(accel: float, v_ego: float, car_fingerprint: str, *, standstill: bool, stopping: bool) -> float:
+  if car_fingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and not (standstill and stopping):
+    return accel
  creep_accel = np.interp(v_ego, [1., 3.], [0.6, 0.])
  creep_accel = np.interp(accel, [0., 0.2], [creep_accel, 0.])
  accel -= creep_accel
@@ -181,12 +183,11 @@ class CarController(CarControllerBase):
    if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
      accel = actuators.accel
      gas = accel
+      stopping = actuators.longControlState == LongCtrlState.stopping

      if CC.longActive:
-        # Compensate for engine creep at low speed.
-        # Either the ABS does not account for engine creep, or the correction is very slow
-        # TODO: verify this applies to EV/hybrid
-        accel = apply_creep_compensation(accel, CS.out.vEgo)
+        accel = apply_creep_compensation(accel, CS.out.vEgo, self.CP.carFingerprint,
+                                         standstill=CS.out.standstill, stopping=stopping)

        # The stock system has been seen rate limiting the brake accel to 5 m/s^3,
        # however even 3.5 m/s^3 causes some overshoot with a step response.
@@ -210,7 +211,6 @@ class CarController(CarControllerBase):
      elif accel_pitch_compensated < 0.0:
        self.brake_request = True

-      stopping = CC.actuators.longControlState == LongCtrlState.stopping
      # TODO: look into using the actuators packet to send the desired speed
      can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX))

@@ -29,6 +29,9 @@ class CarState(CarStateBase):

    self.distance_button = 0
    self.lc_button = 0
+    self.cancel_button = False
+    self.cancel_resume_pressed = False
+    self.cancel_resume_is_cancel = False
    self.lkas_available = False
    self.lateral_motion_control = None
    self.lateral_control_status = None
@@ -156,6 +159,14 @@ class CarState(CarStateBase):
    prev_lc_button = self.lc_button
    self.distance_button = cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"]
    self.lc_button = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
+    prev_cancel_button = self.cancel_button
+    cancel_resume_pressed = bool(cp.vl["Steering_Data_FD1"]["CcAslButtnCnclResPress"])
+    if cancel_resume_pressed and not self.cancel_resume_pressed:
+      self.cancel_resume_is_cancel = ret.cruiseState.available and ret.cruiseState.enabled
+    self.cancel_resume_pressed = cancel_resume_pressed
+    self.cancel_button = bool(cp.vl["Steering_Data_FD1"]["CcAslButtnCnclPress"]) or (
+      cancel_resume_pressed and self.cancel_resume_is_cancel
+    )

    # lock info
    ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"],
@@ -183,10 +194,13 @@ class CarState(CarStateBase):
      except KeyError:
        self.lateral_motion_control = None

-    ret.buttonEvents = [
+    button_events = [
      *create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}),
      *create_button_events(self.lc_button, prev_lc_button, {1: ButtonType.lkas}),
    ]
+    if self.CP.openpilotLongitudinalControl:
+      button_events += create_button_events(self.cancel_button, prev_cancel_button, {1: ButtonType.cancel})
+    ret.buttonEvents = button_events

    fp_ret = custom.StarPilotCarState.new_message()
    fp_ret.brakeLights = ret.brakePressed
@@ -9,12 +9,13 @@ import pytest
 from opendbc.car import Bus, gen_empty_fingerprint
 from opendbc.can import CANPacker
 from opendbc.car.ford import fordcan
-from opendbc.car.ford.carcontroller import FordStockCruiseButton
+from opendbc.car.ford.carcontroller import FordStockCruiseButton, apply_creep_compensation
+from opendbc.car.ford.carstate import CarState
 from opendbc.car.gps import FORD_MACH_E_GPS_MESSAGES, get_car_gps_config, parse_ford_can_gps
-from opendbc.car.structs import CarParams
+from opendbc.car.structs import CarParams, CarState as CarStateStruct
 from opendbc.car.fw_versions import build_fw_dict
 from opendbc.car.ford.interface import CarInterface
-from opendbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, FordSafetyFlags, get_platform_codes, match_vin_to_car
+from opendbc.car.ford.values import CAR, DBC, FW_QUERY_CONFIG, FW_PATTERN, FordSafetyFlags, get_platform_codes, match_vin_to_car
 from opendbc.car.ford.fingerprints import FW_VERSIONS

 Ecu = CarParams.Ecu
@@ -38,6 +39,57 @@ def test_stock_cruise_button_ignores_press_with_cruise_master_off():
  assert button.update(True, cruise_available=False, cruise_enabled=False) == (False, False)


+@pytest.mark.parametrize("op_long", (False, True))
+@pytest.mark.parametrize("standstill", (False, True))
+@pytest.mark.parametrize("cruise_status", (4, 5))
+@pytest.mark.parametrize("switch", ("CcAslButtnCnclResPress", "CcAslButtnCnclPress"))
+def test_ford_cancel_event_does_not_require_pcm_disengagement(op_long, standstill, cruise_status, switch):
+  CP = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], op_long, False, False, None)
+  state = CarState(CP, None)
+  parsers = state.get_can_parsers(CP)
+  packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
+
+  for frame, (pressed, status) in enumerate(((True, cruise_status), (True, 3), (False, 3)), start=1):
+    messages = [
+      packer.make_can_msg("Steering_Data_FD1", fordcan.CanBus(CP).main, {switch: int(pressed)}),
+      packer.make_can_msg("EngBrakeData", fordcan.CanBus(CP).main, {"CcStat_D_Actl": status}),
+      packer.make_can_msg("DesiredTorqBrk", fordcan.CanBus(CP).main, {"VehStop_D_Stat": int(standstill)}),
+    ]
+    parsers[Bus.pt].update([(frame * 100_000_000, messages)])
+    ret, _ = state.update(parsers, None)
+    assert ret.standstill == standstill
+    expected = [(CarStateStruct.ButtonEvent.Type.cancel, pressed)] if op_long and frame != 2 else []
+    assert [(event.type, event.pressed) for event in ret.buttonEvents] == expected
+
+
+@pytest.mark.parametrize("initial_status", (0, 3))
+def test_ford_resume_does_not_become_cancel_when_pcm_engages(initial_status):
+  CP = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], True, False, False, None)
+  state = CarState(CP, None)
+  parsers = state.get_can_parsers(CP)
+  packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
+
+  for frame, (pressed, status) in enumerate(((True, initial_status), (True, 4), (False, 4)), start=1):
+    messages = [
+      packer.make_can_msg("Steering_Data_FD1", fordcan.CanBus(CP).main, {"CcAslButtnCnclResPress": int(pressed)}),
+      packer.make_can_msg("EngBrakeData", fordcan.CanBus(CP).main, {"CcStat_D_Actl": status}),
+    ]
+    parsers[Bus.pt].update([(frame * 100_000_000, messages)])
+    ret, _ = state.update(parsers, None)
+    assert not ret.buttonEvents
+
+
+def test_mach_e_does_not_apply_engine_creep_compensation():
+  for accel in (-1.0, -0.1, 0.0, 0.1):
+    assert apply_creep_compensation(accel, 0.5, CAR.FORD_MUSTANG_MACH_E_MK1,
+                                     standstill=False, stopping=False) == accel
+
+  assert apply_creep_compensation(0.0, 0.0, CAR.FORD_MUSTANG_MACH_E_MK1,
+                                   standstill=True, stopping=True) == -0.6
+  assert apply_creep_compensation(0.0, 0.5, CAR.FORD_F_150_MK14,
+                                   standstill=False, stopping=False) == -0.6
+
+
 ECU_ADDRESSES = {
  Ecu.eps: 0x730,          # Power Steering Control Module (PSCM)
  Ecu.abs: 0x760,          # Anti-Lock Brake System (ABS)
@@ -1065,21 +1065,27 @@ FW_VERSIONS = {
  CAR.HONDA_CITY_7G: {
    (Ecu.eps, 0x18da30f1, None): [
      b'39990-T14-B030\x00\x00',
+      b'39990-T14-B510\x00\x00',
    ],
    (Ecu.gateway, 0x18daeff1, None): [
      b'38897-T14-M110\x00\x00',
+      b'38897-T14-M210\x00\x00',
    ],
    (Ecu.srs, 0x18da53f1, None): [
      b'77959-T00-B830\x00\x00',
+      b'77959-T14-B810\x00\x00',
    ],
    (Ecu.fwdRadar, 0x18dab0f1, None): [
      b'36161-T14-P050\x00\x00',
+      b'8S102-T14-P020\x00\x00',
    ],
    (Ecu.vsa, 0x18da28f1, None): [
      b'57114-T14-B030\x00\x00',
+      b'57114-T14-M510\x00\x00',
    ],
    (Ecu.transmission, 0x18da1ef1, None): [
      b'28101-63B-M420\x00\x00',
+      b'28101-63B-M510\x00\x00',
    ],
  },
  CAR.HONDA_PASSPORT_4G: {
@@ -277,7 +277,7 @@ class CAR(Platforms):
    flags=HondaFlags.BOSCH_RADARLESS,
  )
  HONDA_CITY_7G = HondaBoschPlatformConfig(
-    [HondaCarDocs("Honda City (Brazil only) 2023", "All")],
+    [HondaCarDocs("Honda City (Brazil only) 2023-25", "All")],
    CarSpecs(mass=3125 * CV.LB_TO_KG, wheelbase=2.6, steerRatio=19.0, centerToFrontRatio=0.41, minSteerSpeed=23. * CV.KPH_TO_MS),
    {Bus.pt: 'honda_bosch_radarless_generated'},
    flags=HondaFlags.BOSCH_RADARLESS,
@@ -770,7 +770,8 @@ class CarController(CarControllerBase):
    lead_data = self._can_lead_data.update(lead_distance, lead_rel_speed, lead_visible)

    # HUD messages
-    sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
+    stinger_hud_enabled = CC.enabled or (self.CP.carFingerprint == CAR.KIA_STINGER_2022 and CC.latActive)
+    sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(stinger_hud_enabled, self.car_fingerprint,
                                                                                      hud_control)

    if blended_hda2:
@@ -1042,9 +1043,12 @@ class CarController(CarControllerBase):
            left_sound_active=left_warning.sound_active, right_sound_active=right_warning.sound_active,
          )
        else:
+          host_speed = getattr(CS.out, "vEgoRaw", None) \
+            if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN else None
          adrv_messages = hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame,
                                                             car_fingerprint=self.CP.carFingerprint,
-                                                             drive_gear=drive_gear)
+                                                             drive_gear=drive_gear,
+                                                             v_ego=host_speed)
        can_sends.extend(adrv_messages)
        # The front radar treats ADAS_DRV's 0x100 broadcast as its host heartbeat
        # and stops publishing object tracks when it disappears.
@@ -1335,6 +1335,7 @@ FW_VERSIONS = {
    (Ecu.fwdCamera, 0x7c4, None): [
      b'\xf1\x00CL4 MFC  AT CAN LHD 1.00 1.02 99210-GG000 240708',
      b'\xf1\x00CL4 MFC  AT USA LHD 1.00 1.02 99210-GG000 240708',
+      b'\xf1\x00CL4 MFC  AT USA LHD 1.00 1.04 99210-GG100 251205',
    ],
    (Ecu.fwdRadar, 0x7d0, None): [
      b'\xf1\x00CL4_ RDR -----      1.00 1.01 99110-GG000         ',
@@ -12,7 +12,7 @@ _adrv_0x51_templates: dict[CAR, bytes] = {}


 def cache_adrv_0x51_template(car_fingerprint: CAR, dat: bytes | None) -> None:
-  if car_fingerprint != CAR.KIA_EV6:
+  if car_fingerprint not in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN):
    return

  if dat is None:
@@ -21,15 +21,18 @@ def cache_adrv_0x51_template(car_fingerprint: CAR, dat: bytes | None) -> None:
    _adrv_0x51_templates[car_fingerprint] = bytes(dat)


-def create_adrv_0x51(packer, CAN, frame: int, car_fingerprint: CAR | None = None, drive_gear: bool = False):
+def create_adrv_0x51(packer, CAN, frame: int, car_fingerprint: CAR | None = None, drive_gear: bool = False,
+                     v_ego: float | None = None):
  template = _adrv_0x51_templates.get(car_fingerprint)
  if template is None:
    return packer.make_can_msg("ADRV_0x51", CAN.ACAN, {})

-  # EV6 MRR30 tracks stop when the ADAS takeover replaces this platform payload with zeros.
  dat = bytearray(template)
  dat[2] = (template[2] + frame + 1) & 0xFF
  dat[3] = (dat[3] & ~0x1) | int(drive_gear)
+  if car_fingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN and v_ego is not None and np.isfinite(v_ego):
+    speed_raw = int(np.clip(round(v_ego * 100.0), 0, 65534))
+    dat[8:10] = speed_raw.to_bytes(2, "little")
  crc = hkg_can_fd_checksum(0x51, None, dat)
  dat[0] = crc & 0xFF
  dat[1] = (crc >> 8) & 0xFF
@@ -816,13 +819,14 @@ def create_fca_warning_light(packer, CAN, frame):
  return ret


-def create_adrv_messages(packer, CAN, frame, blended_hda2=False, car_fingerprint=None, drive_gear=False):
+def create_adrv_messages(packer, CAN, frame, blended_hda2=False, car_fingerprint=None, drive_gear=False,
+                         v_ego=None):
  # messages needed to car happy after disabling
  # the ADAS Driving ECU to do longitudinal control

  ret = []

-  ret.append(create_adrv_0x51(packer, CAN, frame, car_fingerprint, drive_gear))
+  ret.append(create_adrv_0x51(packer, CAN, frame, car_fingerprint, drive_gear, v_ego))

  if blended_hda2:
    return ret
@@ -395,7 +395,7 @@ class CarInterface(CarInterfaceBase):

      if not skip_disable_ecu:
        disable_can_recv = can_recv
-        if CP.carFingerprint == CAR.KIA_EV6 and can_recv is not None:
+        if CP.carFingerprint in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN) and can_recv is not None:
          hyundaicanfd.cache_adrv_0x51_template(CP.carFingerprint, None)
          base_can_recv = can_recv
          adrv_bus = CanBus(CP).ACAN
@@ -1316,6 +1316,103 @@ class TestHyundaiFingerprint:
    assert canfd_alt_buttons_cp.flags & HyundaiFlags.CANFD_ALT_BUTTONS
    assert not canfd_alt_buttons_fpcp.redneckCruiseAvailable

+  def test_sportage_hev_hda2_redneck_uses_stock_scc(self, monkeypatch):
+    class FakeParams:
+      def __init__(self, *args, **kwargs):
+        pass
+
+      @staticmethod
+      def get_bool(key):
+        return key == "RedneckCruise"
+
+    monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams)
+    toggles = get_test_toggles()
+    fingerprint = gen_empty_fingerprint()
+    can_bus = CanBus(None, fingerprint, True)
+    fingerprint[can_bus.CAM][0x110] = 32
+    fingerprint[can_bus.ECAN][0x1CF] = 8
+
+    CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
+    FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
+
+    assert CP.flags & HyundaiFlags.CANFD_LKA_STEERING
+    assert FPCP.redneckCruiseAvailable
+    assert not FPCP.pcmCruiseSpeed
+    assert CP.pcmCruise
+    assert not CP.openpilotLongitudinalControl
+    assert not CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG
+    controller = CarInterface(CP, FPCP).CC
+    assert not controller.long_active_ecu
+
+    controller.frame = 30
+    CS = SimpleNamespace(redneck_send_button=1, buttons_counter=5)
+    msgs = controller._create_canfd_redneck_button_messages(CS)
+    assert len(msgs) == 20
+    assert all(msg[0] == 0x1CF and msg[2] == can_bus.ECAN for msg in msgs)
+    assert all(msg[1][2] & 0x7 == Buttons.RES_ACCEL for msg in msgs)
+
+    controller.frame = 60
+    CS.redneck_send_button = 2
+    msgs = controller._create_canfd_redneck_button_messages(CS)
+    assert len(msgs) == 20
+    assert all(msg[1][2] & 0x7 == Buttons.SET_DECEL for msg in msgs)
+
+    monkeypatch.setattr(FakeParams, "get_bool", staticmethod(lambda key: False))
+    CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
+    FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
+    assert FPCP.redneckCruiseAvailable
+    assert FPCP.pcmCruiseSpeed
+    assert CP.pcmCruise
+    assert not CP.openpilotLongitudinalControl
+
+  def test_sportage_redneck_rejects_unverified_button_layouts(self, monkeypatch):
+    class FakeParams:
+      def __init__(self, *args, **kwargs):
+        pass
+
+      @staticmethod
+      def get_bool(key):
+        return key == "RedneckCruise"
+
+    monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams)
+    toggles = get_test_toggles()
+    for button_address, button_bus, button_length, lka_steering in (
+      (0x1AA, 1, 16, True),
+      (0x1CF, 0, 8, True),
+      (0x1CF, 0, 8, False),
+      (0x1CF, 1, 16, True),
+    ):
+      fingerprint = gen_empty_fingerprint()
+      can_bus = CanBus(None, fingerprint, lka_steering)
+      if lka_steering:
+        fingerprint[can_bus.CAM][0x110] = 32
+      fingerprint[button_bus][button_address] = button_length
+
+      CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
+      FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
+      assert not FPCP.redneckCruiseAvailable
+      assert FPCP.pcmCruiseSpeed
+      assert CP.pcmCruise
+      assert not CP.openpilotLongitudinalControl
+
+    fingerprint = gen_empty_fingerprint()
+    can_bus = CanBus(None, fingerprint, True)
+    fingerprint[can_bus.CAM][0x110] = 32
+    fingerprint[can_bus.ECAN][0x1CF] = 8
+    fingerprint[can_bus.ECAN][0x1AA] = 16
+    CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
+    FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
+    assert not FPCP.redneckCruiseAvailable
+
+    fingerprint = gen_empty_fingerprint()
+    can_bus = CanBus(None, fingerprint, True)
+    fingerprint[can_bus.CAM][0x110] = 32
+    fingerprint[can_bus.ECAN][0x1CF] = 8
+    CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
+    CP.openpilotLongitudinalControl = True
+    FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
+    assert not FPCP.redneckCruiseAvailable
+
  def test_hyundai_non_scc_without_redneck_keeps_stock_longitudinal_mode(self, monkeypatch):
    class FakeParams:
      def __init__(self, *args, **kwargs):
@@ -1699,6 +1796,24 @@ class TestHyundaiFingerprint:
    assert exact
    assert matches == {candidate}

+  @pytest.mark.parametrize("camera_fw", [
+    b'\xf1\x00CL4 MFC  AT CAN LHD 1.00 1.02 99210-GG000 240708',
+    b'\xf1\x00CL4 MFC  AT USA LHD 1.00 1.02 99210-GG000 240708',
+    b'\xf1\x00CL4 MFC  AT USA LHD 1.00 1.04 99210-GG100 251205',
+  ])
+  @pytest.mark.parametrize("radar_fw", [
+    b'\xf1\x00CL4_ RDR -----      1.00 1.01 99110-GG000         ',
+    b'\xf1\x00CL4_ RDR -----      1.00 1.01 99110-GG100         ',
+  ])
+  def test_k4_2025_2026_fw_exact_matches(self, camera_fw, radar_fw):
+    car_fw = [
+      CarParams.CarFw(ecu=Ecu.fwdCamera, fwVersion=camera_fw, address=0x7c4, brand="hyundai"),
+      CarParams.CarFw(ecu=Ecu.fwdRadar, fwVersion=radar_fw, address=0x7d0, brand="hyundai"),
+    ]
+    exact, matches = match_fw_to_car(car_fw, "", allow_exact=True, allow_fuzzy=False, log=False)
+    assert exact
+    assert matches == {CAR.KIA_K4_2025}
+
  def test_staria_2023_australian_route_fw_exact_matches(self):
    route_fw = {
      (Ecu.fwdCamera, 0x7c4): b'\xf1\x00US4 MFC  AT AUS RHD 1.00 1.04 99211-CG000 210819',
@@ -605,7 +605,7 @@ class CAR(Platforms):
  )
  KIA_K4_2025 = HyundaiCanFDPlatformConfig(
    [
-      HyundaiCarDocs("Kia K4 (without HDA II) 2025", car_parts=CarParts.common([CarHarness.hyundai_a])),
+      HyundaiCarDocs("Kia K4 (without HDA II) 2025-26", car_parts=CarParts.common([CarHarness.hyundai_a])),
      HyundaiCarDocs("Kia K4 (with HDA II) 2025", car_parts=CarParts.common([CarHarness.hyundai_r])),
    ],
    CarSpecs(mass=2987 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=13.4),
@@ -248,12 +248,24 @@ class CarInterfaceBase(ABC):
            (candidate != HYUNDAI.KIA_STINGER_2022 or fingerprint[2][0x53E] == 6):
          fp_ret.flags |= HyundaiStarPilotFlags.HAS_LKAS12.value

-        fp_ret.redneckCruiseAvailable = (bool(CP.flags & HyundaiFlags.NON_SCC) and
-                                         not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
-                                         not (candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED))
+        sportage_stock_scc_buttons = (
+          candidate == HYUNDAI.KIA_SPORTAGE_HEV_2026 and
+          not CP.openpilotLongitudinalControl and
+          bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING) and
+          not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
+          fingerprint[CAN.ECAN].get(0x1CF) == 8 and
+          0x1AA not in fingerprint[CAN.ECAN]
+        )
+        fp_ret.redneckCruiseAvailable = (
+          (bool(CP.flags & HyundaiFlags.NON_SCC) and
+           not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
+           not (candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED)) or
+          sportage_stock_scc_buttons
+        )
        if fp_ret.redneckCruiseAvailable and params.get_bool("RedneckCruise"):
          fp_ret.pcmCruiseSpeed = False
-          CP.openpilotLongitudinalControl = True
+          if CP.flags & HyundaiFlags.NON_SCC:
+            CP.openpilotLongitudinalControl = True

        if candidate == HYUNDAI.HYUNDAI_ELANTRA_HEV_2024 and CP.openpilotLongitudinalControl:
          fp_ret.flags |= HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING.value
@@ -48,6 +48,7 @@ class CarController(CarControllerBase):
    self.angle_handoff_active = False
    self.ascent_angle_initialized = False
    self.ascent_aol_arm_frames = 0
+    self.ascent_es_distance_counter_last = None

    self.cruise_button_prev = 0
    self.steer_rate_counter = 0
@@ -411,7 +412,12 @@ class CarController(CarControllerBase):
          can_sends.append(subarucan.create_es_distance(self.packer, self.frame // 5, CS.es_distance_msg, 0, pcm_cancel_cmd,
                                                        self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle))
      else:
-        if pcm_cancel_cmd:
+        cancel_frame_ready = True
+        if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
+          stock_counter = CS.es_distance_msg["COUNTER"]
+          cancel_frame_ready = stock_counter != self.ascent_es_distance_counter_last
+          self.ascent_es_distance_counter_last = stock_counter
+        if pcm_cancel_cmd and cancel_frame_ready:
          if not (self.CP.flags & SubaruFlags.HYBRID):
            bus = CanBus.alt_for_cp(self.CP) if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else self.main_bus
            can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd))
@@ -879,6 +879,69 @@ def test_ascent_hud_waits_for_angle_request():
  assert controller._lkas_status_active(CC)


+@pytest.mark.parametrize("platform", [CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023, CAR.SUBARU_LEGACY_2025,
+                                      CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_ASCENT])
+def test_stock_cruise_cancel_fresh_frame_gate_is_ascent_angle_only(platform):
+  CP = CarInterface.get_non_essential_params(platform)
+  controller = CarController({}, CP)
+  CC = structs.CarControl()
+  CC.cruiseControl.cancel = True
+  CS = SimpleNamespace(
+    out=structs.CarState(),
+    es_distance_msg=defaultdict(int, COUNTER=11),
+    es_dashstatus_msg=defaultdict(int),
+    es_lkas_state_msg=defaultdict(int),
+    es_infotainment_msg=defaultdict(int),
+  )
+  toggles = SimpleNamespace(subaru_sng=False)
+  cancel_messages = []
+  for frame in range(5):
+    _, sends = controller.update(CC.as_reader(), CS, frame * 10_000_000, toggles)
+    cancel_messages.extend(msg for msg in sends if msg[0] == 0x221)
+
+  assert len(cancel_messages) == (1 if platform == CAR.SUBARU_ASCENT_2023 else 5)
+  assert all(msg[2] == (CanBus.alt if CP.flags & SubaruFlags.GLOBAL_GEN2 else CanBus.main) for msg in cancel_messages)
+  parser = CANParser(DBC[platform][Bus.pt], [("ES_Distance", 0)], cancel_messages[0][2])
+  parser.update([(1, [cancel_messages[0]])])
+  assert parser.vl["ES_Distance"]["COUNTER"] == 12
+  assert parser.vl["ES_Distance"]["Cruise_Cancel"] == 1
+  assert parser.vl["ES_Distance"]["Cruise_Throttle"] == 1818
+
+
+def test_ascent_cancel_uses_fresh_stock_frames_and_handles_counter_rollover():
+  CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
+  controller = CarController({}, CP)
+  CC = structs.CarControl()
+  CC.latActive = True
+  CS = SimpleNamespace(
+    out=structs.CarState(vEgoRaw=24.04, steeringAngleDeg=2.93, gearShifter="drive"),
+    es_distance_msg=defaultdict(int, COUNTER=14),
+    es_dashstatus_msg=defaultdict(int),
+    es_lkas_state_msg=defaultdict(int),
+    es_infotainment_msg=defaultdict(int),
+  )
+  CS.out.cruiseState.available = True
+  toggles = SimpleNamespace(subaru_sng=False)
+  parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_Distance", 0)], CanBus.alt)
+  cancel_counters = []
+  for frame, (stock_counter, cancel) in enumerate([
+    (14, False), (14, True), (15, True), (15, True), (0, True), (0, True),
+    (1, False), (1, True), (2, True),
+  ]):
+    CS.es_distance_msg["COUNTER"] = stock_counter
+    CC.cruiseControl.cancel = cancel
+    _, sends = controller.update(CC.as_reader(), CS, frame * 10_000_000, toggles)
+    cancel_messages = [msg for msg in sends if msg[0] == 0x221]
+    assert len(cancel_messages) <= 1
+    if cancel_messages:
+      parser.update([(frame + 1, cancel_messages)])
+      cancel_counters.append(parser.vl["ES_Distance"]["COUNTER"])
+      assert parser.vl["ES_Distance"]["Cruise_Cancel"] == 1
+      assert parser.vl["ES_Distance"]["Cruise_Throttle"] == 1818
+
+  assert cancel_counters == [0, 1, 3]
+
+
 def test_other_angle_cars_keep_lateral_status_behavior():
  CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
  controller = CarController({}, CP)
@@ -10,7 +10,7 @@ from opendbc.car.secoc import add_mac, build_sync_mac
 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, \
+                                        CarControllerParams, ToyotaFlags, ToyotaSafetyFlags, \
                                        UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, TOYOTA_AUTO_HOLD_AEB_CARS
 from opendbc.can import CANPacker

@@ -68,6 +68,11 @@ def is_ths_hybrid(CP) -> bool:
  return CP.carFingerprint in LEGACY_PRIUS_CAR or is_camry_hybrid(CP)


+def uses_rav4_hybrid_sdsu_longitudinal(CP) -> bool:
+  return bool(CP.carFingerprint == CAR.TOYOTA_RAV4H and CP.openpilotLongitudinalControl and
+              CP.safetyConfigs[0].safetyParam & ToyotaSafetyFlags.LONG_FILTER.value)
+
+
 def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
  highlander_sdsu = (
    CP.carFingerprint == CAR.TOYOTA_HIGHLANDER and
@@ -107,7 +112,7 @@ def get_long_tune(CP, params):
  kiV = [0.5, 0.25]
  k_f = 1.0

-  if is_ths_hybrid(CP):
+  if is_ths_hybrid(CP) or uses_rav4_hybrid_sdsu_longitudinal(CP):
    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.]
@@ -1,6 +1,6 @@
 from opendbc.car import Bus, structs, get_safety_config, uds
 from opendbc.car.toyota.carstate import CarState
-from opendbc.car.toyota.carcontroller import CarController
+from opendbc.car.toyota.carcontroller import CarController, uses_rav4_hybrid_sdsu_longitudinal
 from opendbc.car.toyota.radar_interface import RadarInterface
 from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
                                                  MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \
@@ -176,7 +176,8 @@ class CarInterface(CarInterfaceBase):

    # min speed to enable ACC. if car can do stop and go, then set enabling speed
    # to a negative value, so it won't matter.
-    ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptorDEPRECATED) else MIN_ACC_SPEED
+    rav4_hybrid_sdsu_long_defaults = uses_rav4_hybrid_sdsu_longitudinal(ret)
+    ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptorDEPRECATED or rav4_hybrid_sdsu_long_defaults) else MIN_ACC_SPEED

    prius_long_defaults = candidate in LEGACY_PRIUS_CAR and ret.openpilotLongitudinalControl
    camry_hybrid_long_defaults = (candidate == CAR.TOYOTA_CAMRY and ret.openpilotLongitudinalControl and
@@ -193,7 +194,7 @@ class CarInterface(CarInterfaceBase):
      if ret.flags & ToyotaFlags.HYBRID.value:
        ret.longitudinalActuatorDelay = 0.05

-    if camry_hybrid_long_defaults:
+    if camry_hybrid_long_defaults or rav4_hybrid_sdsu_long_defaults:
      # The THS eCVT responds much faster than the legacy non-TSS2 ICE tune.
      ret.longitudinalActuatorDelay = 0.05
      ret.vEgoStopping = 0.25
@@ -992,6 +992,22 @@ class TestSportageNoStockLka(unittest.TestCase):
      "Damping_Gain": 100,
    })

+  def test_stock_scc_buttons_require_engagement(self):
+    resume = self.packer.make_can_msg_safety("CRUISE_BUTTONS", 1, {"CRUISE_BUTTONS": Buttons.RESUME})
+    set_button = self.packer.make_can_msg_safety("CRUISE_BUTTONS", 1, {"CRUISE_BUTTONS": Buttons.SET})
+    self.assertFalse(self.safety.safety_tx_hook(resume))
+    self.assertFalse(self.safety.safety_tx_hook(set_button))
+
+    self.safety.safety_rx_hook(set_button)
+    self.safety.safety_rx_hook(self.packer.make_can_msg_safety("SCC_CONTROL", 1, {"ACCMode": 1}))
+    self.assertTrue(self.safety.get_controls_allowed())
+    self.assertTrue(self.safety.safety_tx_hook(resume))
+    self.assertTrue(self.safety.safety_tx_hook(set_button))
+
+    self.safety.safety_rx_hook(self.packer.make_can_msg_safety("SCC_CONTROL", 1, {"ACCMode": 0}))
+    self.assertFalse(self.safety.safety_tx_hook(resume))
+    self.assertFalse(self.safety.safety_tx_hook(set_button))
+
  def test_aol_toggle_keeps_stock_blocked_and_inactive_status_allowed(self):
    self._speed(30)
    for expected_aol in (False, True, False, True, False):
@@ -197,7 +197,9 @@ class RedneckCruise:
  def _update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None:
    update_manual_button_timers(CS, self.cruise_button_timers)
    button_pressed = any(0 < timer <= int(MANUAL_BUTTON_INACTIVE_TIMER / DT_CTRL) for timer in self.cruise_button_timers.values())
-    self.is_ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume and not button_pressed
+    stock_cruise_ready = (not self.CP.pcmCruise or self.CP.openpilotLongitudinalControl or CS.cruiseState.enabled)
+    self.is_ready = (CC.enabled and stock_cruise_ready and not CC.cruiseControl.override and
+                     not CC.cruiseControl.cancel and not CC.cruiseControl.resume and not button_pressed)

  def _desired_state(self) -> str:
    if self.v_target > self.v_cruise_cluster:
@@ -26,13 +26,13 @@ ButtonType = car.CarState.ButtonEvent.Type

 class TestRedneckCruise(unittest.TestCase):
  def setUp(self):
-    self.CP = SimpleNamespace()
+    self.CP = SimpleNamespace(pcmCruise=False, openpilotLongitudinalControl=False)
    self.FPCP = SimpleNamespace(pcmCruiseSpeed=False, redneckCruiseAvailable=True)
    self.redneck = RedneckCruise(self.CP, self.FPCP)

-  def _new_state(self, speed_cluster_mph=20.0, button_events=None):
+  def _new_state(self, speed_cluster_mph=20.0, button_events=None, cruise_enabled=True):
    return SimpleNamespace(
-      cruiseState=SimpleNamespace(speedCluster=speed_cluster_mph * CV.MPH_TO_MS),
+      cruiseState=SimpleNamespace(speedCluster=speed_cluster_mph * CV.MPH_TO_MS, enabled=cruise_enabled),
      buttonEvents=button_events or [],
    )

@@ -143,6 +143,21 @@ class TestRedneckCruise(unittest.TestCase):
        send_button, _ = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0, **kwargs)
        self.assertEqual(SEND_BUTTON_NONE, send_button)

+  def test_stock_scc_only_sends_buttons_while_engaged(self):
+    self.CP.pcmCruise = True
+    frames = int(INCREASE_INACTIVE_TIMER / DT_CTRL) + 4
+    for _ in range(frames):
+      send_button, _ = self.redneck.run(self._new_state(cruise_enabled=False), self._new_control(),
+                                        25.0 * CV.MPH_TO_MS, is_metric=False)
+      self.assertEqual(SEND_BUTTON_NONE, send_button)
+
+    send_button, _ = self._run_until_active(target_mph=25.0)
+    self.assertEqual(SEND_BUTTON_INCREASE, send_button)
+
+    send_button, _ = self.redneck.run(self._new_state(cruise_enabled=False), self._new_control(),
+                                      25.0 * CV.MPH_TO_MS, is_metric=False)
+    self.assertEqual(SEND_BUTTON_NONE, send_button)
+
  def test_resets_when_pcm_cruise_speed_is_enabled(self):
    self.FPCP.pcmCruiseSpeed = True
    send_button, v_target = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0)
@@ -31,7 +31,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from openpilot.selfdrive.controls.lib.steering_saturation import is_angle_steering_limited
 from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature
-from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import GENESIS_GV70_CARS, GenesisGV70HighwayCommandStabilizer
+from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import get_genesis_highway_command_stabilizer
 from openpilot.selfdrive.controls.lib.latcontrol_torque import (
  BOLT_2018_2021_STEER_RATIO_TEST_SCALE,
  LatControlTorque,
@@ -427,9 +427,8 @@ class Controls:
      self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL)
    elif self.CP.lateralTuning.which() == 'torque':
      self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL)
-    self.gv70_highway_stabilizer = (GenesisGV70HighwayCommandStabilizer()
-                                    if self.CP.carFingerprint in GENESIS_GV70_CARS and self.CP.lateralTuning.which() == 'torque'
-                                    else None)
+    self.genesis_highway_stabilizer = get_genesis_highway_command_stabilizer(
+      self.CP.carFingerprint, self.CP.lateralTuning.which() == 'torque')

    self.sm = self.sm.extend(['liveDelay', 'starpilotCarState', 'starpilotPlan'])

@@ -751,14 +750,14 @@ class Controls:
      bool(CS.leftBlinker or CS.rightBlinker),
      bool(CS.steeringPressed))

-    if self.gv70_highway_stabilizer is not None:
-      stabilize_gv70 = (CC.latActive and isinstance(self.LaC, LatControlTorque) and
-                       not CS.steeringPressed and not CS.leftBlinker and not CS.rightBlinker and
-                       not self.starpilot_toggles.lane_centering and
-                       model_v2.meta.laneChangeState == LaneChangeState.off and
-                       self.sm.all_checks(['modelV2']))
-      new_desired_curvature = self.gv70_highway_stabilizer.update(
-        new_desired_curvature, CS.vEgo, stabilize_gv70, DT_CTRL)
+    if self.genesis_highway_stabilizer is not None:
+      stabilize_genesis = (CC.latActive and isinstance(self.LaC, LatControlTorque) and
+                           not CS.steeringPressed and not CS.leftBlinker and not CS.rightBlinker and
+                           not self.starpilot_toggles.lane_centering and
+                           model_v2.meta.laneChangeState == LaneChangeState.off and
+                           self.sm.all_checks(['modelV2']))
+      new_desired_curvature = self.genesis_highway_stabilizer.update(
+        new_desired_curvature, CS.vEgo, stabilize_genesis, DT_CTRL)

    jerk_factor = 1.0
    if self.starpilot_toggles.lane_change_pace < 10:
@@ -23,6 +23,9 @@ _CENTER_ERROR_DEADBAND = 0.08
 _E2E_MAX_PATH_STD = 0.35
 _E2E_BREAK_IN_START = 0.15
 _E2E_BREAK_IN_FULL = 0.50
+_E2E_MIN_LANE_AUTHORITY = 0.20
+_E2E_BOUNDARY_LANE_AUTHORITY = 0.50
+_E2E_BOUNDARY_MARGIN = 0.40


 class LaneCenteringController:
@@ -146,7 +149,16 @@ class LaneCenteringController:
              0.0,
              1.0,
            )
-            error *= 1.0 - e2e_authority * float(break_in)
+            path_clearance = min(model_y - left, right - model_y)
+            boundary_weight = float(np.clip(
+              (_MIN_CENTER_TO_LINE + _E2E_BOUNDARY_MARGIN - path_clearance) / _E2E_BOUNDARY_MARGIN,
+              0.0,
+              1.0,
+            ))
+            lane_authority = _E2E_MIN_LANE_AUTHORITY + boundary_weight * (
+              _E2E_BOUNDARY_LANE_AUTHORITY - _E2E_MIN_LANE_AUTHORITY
+            )
+            error *= 1.0 - e2e_authority * float(break_in) * (1.0 - lane_authority)
      except (AttributeError, TypeError, ValueError):
        pass

@@ -280,7 +280,9 @@ GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [0.75, 1.0]
 GENESIS_GV70_HIGHWAY_STABILIZER_BASELINE_RC = 0.85
 GENESIS_GV70_HIGHWAY_STABILIZER_BLEND_RC = 0.35
 GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_LAT = 0.06
-GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_WINDOW = 4.0
+GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_WINDOW = 6.0
+GENESIS_GV70_HIGHWAY_STABILIZER_RECOVERY_SECONDS = 4.0
+GENESIS_GV70_HIGHWAY_STABILIZER_DIRECTION_CHANGE_LAT = 0.35
 GENESIS_GV70_HIGHWAY_STABILIZER_REDUCTION = 0.70
 GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA = 0.20

@@ -362,6 +364,8 @@ GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC = 0.18
 GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC = 0.10
 GENESIS_G70_OUTPUT_SMOOTHING_RELEASE_RC = 0.03
 GENESIS_G70_OUTPUT_SMOOTHING_OVERSHOOT = 0.08
+GENESIS_G70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [1.0, 1.4]
+GENESIS_G70_HIGHWAY_STABILIZER_CURVE_EXIT_LAT = 0.15
 GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45
 GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0
 GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0
@@ -3305,8 +3309,10 @@ def get_genesis_gv70_stabilized_output(output_torque: float, prev_output_torque:
  return float(output_torque + speed_weight * (smoothed_output - output_torque))


-class GenesisGV70HighwayCommandStabilizer:
-  def __init__(self) -> None:
+class GenesisHighwayCommandStabilizer:
+  def __init__(self, center_lat_bp: list[float], curve_exit_lat: float = 0.0) -> None:
+    self.center_lat_bp = tuple(center_lat_bp)
+    self.curve_exit_lat = curve_exit_lat
    self.reset()

  def reset(self) -> None:
@@ -3315,6 +3321,8 @@ class GenesisGV70HighwayCommandStabilizer:
    self.reversals: deque[float] = deque()
    self.elapsed = 0.0
    self.blend = 0.0
+    self.active_until = 0.0
+    self.curve_direction = 0

  def update(self, curvature: float, v_ego: float, enabled: bool, dt: float) -> float:
    if not enabled or v_ego <= GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP[0] or not math.isfinite(curvature):
@@ -3325,13 +3333,20 @@ class GenesisGV70HighwayCommandStabilizer:
    lateral_accel = curvature * v_ego ** 2
    if self.baseline is None:
      self.baseline = lateral_accel
+    if abs(self.baseline) >= GENESIS_GV70_HIGHWAY_STABILIZER_DIRECTION_CHANGE_LAT:
+      self.curve_direction = 1 if self.baseline > 0.0 else -1
+    if self.curve_direction * lateral_accel < 0.0:
+      self.reset()
+      self.baseline = lateral_accel
+      return curvature
    self.baseline += dt / (GENESIS_GV70_HIGHWAY_STABILIZER_BASELINE_RC + dt) * (lateral_accel - self.baseline)
    residual = lateral_accel - self.baseline

-    if abs(lateral_accel) >= GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP[1]:
+    if abs(lateral_accel) >= self.center_lat_bp[1]:
      self.last_sign = 0
      self.reversals.clear()
      self.blend = 0.0
+      self.active_until = 0.0
      return curvature

    sign = 0
@@ -3347,15 +3362,38 @@ class GenesisGV70HighwayCommandStabilizer:
      self.reversals.popleft()

    speed_weight = float(np.interp(v_ego, GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP, [0.0, 1.0]))
-    target_blend = speed_weight if len(self.reversals) >= 3 else 0.0
+    if len(self.reversals) >= 3:
+      self.active_until = self.elapsed + GENESIS_GV70_HIGHWAY_STABILIZER_RECOVERY_SECONDS
+    target_blend = speed_weight if self.elapsed < self.active_until else 0.0
    self.blend += dt / (GENESIS_GV70_HIGHWAY_STABILIZER_BLEND_RC + dt) * (target_blend - self.blend)
-    center_weight = float(np.interp(abs(lateral_accel), GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP, [1.0, 0.0]))
+    center_weight = float(np.interp(abs(lateral_accel), self.center_lat_bp, [1.0, 0.0]))
+    if self.curve_direction and self.curve_exit_lat > 0.0:
+      center_weight *= float(np.interp(abs(lateral_accel), [0.0, self.curve_exit_lat], [0.0, 1.0]))
    correction = float(np.clip(GENESIS_GV70_HIGHWAY_STABILIZER_REDUCTION * residual,
                               -GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA,
                               GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA))
    return float((lateral_accel - self.blend * center_weight * correction) / v_ego ** 2)


+class GenesisGV70HighwayCommandStabilizer(GenesisHighwayCommandStabilizer):
+  def __init__(self) -> None:
+    super().__init__(GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP)
+
+
+class GenesisG70HighwayCommandStabilizer(GenesisHighwayCommandStabilizer):
+  def __init__(self) -> None:
+    super().__init__(GENESIS_G70_HIGHWAY_STABILIZER_CENTER_LAT_BP, GENESIS_G70_HIGHWAY_STABILIZER_CURVE_EXIT_LAT)
+
+
+def get_genesis_highway_command_stabilizer(car_fingerprint: str, torque_control: bool) -> GenesisHighwayCommandStabilizer | None:
+  if torque_control:
+    if car_fingerprint in GENESIS_G70_CARS:
+      return GenesisG70HighwayCommandStabilizer()
+    if car_fingerprint in GENESIS_GV70_CARS:
+      return GenesisGV70HighwayCommandStabilizer()
+  return None
+
+
 def get_genesis_g70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
                                       desired_lateral_jerk: float = 0.0) -> float:
  base_threshold = get_standard_friction_threshold(v_ego)
@@ -428,9 +428,11 @@ def gen_long_ocp():


 class LongitudinalMpc:
-  def __init__(self, mode='acc', dt=DT_MDL):
+  def __init__(self, mode='acc', dt=DT_MDL, *, hold_stopped_lead_position=False, sync_model_lead_filters=False):
    self.mode = mode
    self.dt = dt
+    self.hold_stopped_lead_position = hold_stopped_lead_position
+    self.sync_model_lead_filters = sync_model_lead_filters
    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
    self.source = SOURCES[2]
    # Initialize smoothing filters with default time constants
@@ -601,7 +603,7 @@ class LongitudinalMpc:
        self.solver.set(i, 'x', self.x0)

  @staticmethod
-  def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego=0.0):
+  def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego=0.0, *, hold_stopped_lead_position=False):
    speed_mph = v_ego * CV.MS_TO_MPH
    bp = [0, 20, 35]
    exp_weight = np.interp(speed_mph, bp, [1.0, 1.0, 0.0])  # Full exp at <20, blend to constant at 35
@@ -617,7 +619,10 @@ class LongitudinalMpc:

    # Constant acceleration component
    v_lead_traj_const = np.clip(v_lead + a_lead * T_IDXS, 0.0, 1e8)
-    x_lead_traj_const = x_lead + v_lead * T_IDXS + 0.5 * a_lead * T_IDXS**2
+    position_time = T_IDXS
+    if hold_stopped_lead_position and a_lead < 0.0:
+      position_time = np.minimum(T_IDXS, max(v_lead, 0.0) / -a_lead)
+    x_lead_traj_const = x_lead + v_lead * position_time + 0.5 * a_lead * position_time**2

    # Blend based on weight
    v_lead_traj = exp_weight * v_lead_traj_exp + (1 - exp_weight) * v_lead_traj_const
@@ -633,6 +638,18 @@ class LongitudinalMpc:
    if lead_active:
      model_lead_xv = build_model_lead_trajectory(model_lead, lead, v_ego)
      if model_lead_xv is not None:
+        if self.sync_model_lead_filters:
+          a_lead = soften_far_radar_lead_accel(
+            lead.dRel, lead.vLead, lead.aLeadK, v_ego,
+            get_T_FOLLOW() if t_follow is None else t_follow,
+            radar=bool(getattr(lead, "radar", False)),
+          )
+          self.lead_a_filter.update(float(np.clip(a_lead, -10., 5.)))
+          self.lead_v_filter.update(float(np.clip(lead.vLead, 0.0, 1e8)))
+          for lead_filter in (self.duplicate_lead_x_filters[lead_index],
+                              self.duplicate_lead_a_filters[lead_index],
+                              self.duplicate_lead_v_filters[lead_index]):
+            lead_filter.initialized = False
        return model_lead_xv

    if lead_active:
@@ -688,7 +705,8 @@ class LongitudinalMpc:
      self.duplicate_lead_x_filters[lead_index].initialized = False
      self.duplicate_lead_a_filters[lead_index].initialized = False
      self.duplicate_lead_v_filters[lead_index].initialized = False
-    lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego)
+    lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego,
+                                    hold_stopped_lead_position=self.hold_stopped_lead_position)
    return lead_xv

  @staticmethod
@@ -35,6 +35,10 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
  is_gm_silverado_early_follow_lead,
  is_toyota_rav4_tss2_post_departure_tune,
  get_toyota_rav4_tss2_early_lead_cap,
+  get_toyota_corolla_braking_lead_cap,
+  is_toyota_corolla_early_radar_follow_lead,
+  use_stopped_lead_position,
+  use_model_lead_filter_sync,
  is_toyota_rav4_tss2_radar_follow_lead,
  get_toyota_sienna_post_departure_restop_cap,
  get_untracked_slow_lead_decel_scale,
@@ -578,7 +582,8 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
 class LongitudinalPlanner:
  def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
    self.CP = CP
-    self.mpc = LongitudinalMpc(dt=dt)
+    self.mpc = LongitudinalMpc(dt=dt, hold_stopped_lead_position=use_stopped_lead_position(CP),
+                              sync_model_lead_filters=use_model_lead_filter_sync(CP))
    self.fcw = False
    self.dt = dt
    self.model_allow_throttle = True
@@ -2144,7 +2149,9 @@ class LongitudinalPlanner:
    # safety path so ACC/chill does not ignore a visible lead during that debounce.
    lead_control_active = (
      tracking_lead or raw_close_lead_control or early_truck_follow or rav4_radar_follow or
-      lightning_stopped_radar_follow
+      lightning_stopped_radar_follow or
+      any(is_toyota_corolla_early_radar_follow_lead(self.CP, lead, scene_v_ego)
+          for lead in (self.lead_one, self.lead_two))
    )
    lead_one_active = bool(self.lead_one.status and lead_control_active)
    effective_t_follow = self.get_dynamic_t_follow(sm['starpilotPlan'].tFollow, self.lead_one if lead_one_active else None, v_ego)
@@ -2580,6 +2587,16 @@ class LongitudinalPlanner:
    vision_low_speed_stop_active = False
    vision_brake_cap_active = False
    if lead_control_active:
+      if (not experimental_mode and
+          not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and
+          not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and
+          not bool(getattr(sm['starpilotPlan'], 'stopSignConfirmed', False))):
+        corolla_cap = get_toyota_corolla_braking_lead_cap(
+          self.CP, self.lead_one, v_ego,
+          desired_follow_distance(v_ego, self.lead_one.vLead, effective_t_follow), output_accel_min,
+        )
+        if corolla_cap is not None:
+          close_lead_caps.append(corolla_cap)
      for lead in (self.lead_one, self.lead_two):
        rav4_early_lead_cap = get_toyota_rav4_tss2_early_lead_cap(
          self.CP, lead, v_ego, output_accel_min,
@@ -24,6 +24,8 @@ HONDA_ACCORD_STANDSTILL_GUARD_MAX_EGO_SPEED = 0.25
 HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25
 GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE = 1.75
 KIA_NIRO_EV_LEAD_FOLLOW_JERK_SCALE = 1.5
+KIA_NIRO_EV_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5
+KIA_NIRO_EV_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75
 GENESIS_GV70_ELECTRIFIED_SCC_JERK_UPPER = 1.5
 GENESIS_GV70_ELECTRIFIED_SCC_JERK_LOWER = 2.0
 GENESIS_GV70_ELECTRIFIED_SCC_URGENT_JERK_LOWER = 5.0
@@ -66,6 +68,7 @@ TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_MODEL_PROB = 0.95
 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LATERAL_OFFSET = 1.75
 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_BRAKE = 0.18
 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_BRAKE = 0.32
+TOYOTA_COROLLA_BRAKING_LEAD_MAX_DECEL = 1.5
 TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_EGO_SPEED = 12.0
 TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_MODEL_PROB = 0.85
 TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_LATERAL_OFFSET = 1.2
@@ -136,6 +139,7 @@ TOYOTA_CAMRY_TSS2_FORCE_STOP_DISTANCE_BIAS_M = 6.0
 DEFAULT_FORCE_STOP_HANDOFF_M = 6.0
 HYUNDAI_SANTA_FE_2022_FORCE_STOP_REANCHOR_SPEED_TOLERANCE = 0.25
 HYUNDAI_SANTA_FE_2022_FORCE_STOP_LOW_SPEED_HOLD = 2.5
+FORD_MACH_E_FORCE_STOP_LOW_SPEED_HOLD = 1.0
 KIA_CARNIVAL_2025_STOP_SIGN_LOW_SPEED_HOLD = 0.75


@@ -168,6 +172,59 @@ def get_toyota_prius_stopped_lead_obstacle_bias(CP, lead, v_ego):
  return float(min(bias, max(distance - 0.5, 0.0)))


+def use_stopped_lead_position(CP):
+  return (
+    getattr(CP, "brand", "") == "toyota" and
+    str(getattr(CP, "carFingerprint", "")) == "TOYOTA_COROLLA_TSS2"
+  )
+
+
+def use_model_lead_filter_sync(CP):
+  return (
+    getattr(CP, "brand", "") == "toyota" and
+    str(getattr(CP, "carFingerprint", "")) == "TOYOTA_COROLLA_TSS2"
+  )
+
+
+def is_toyota_corolla_early_radar_follow_lead(CP, lead, v_ego):
+  if (
+    getattr(CP, "brand", "") != "toyota" or
+    str(getattr(CP, "carFingerprint", "")) != "TOYOTA_COROLLA_TSS2" or
+    lead is None or not bool(getattr(lead, "status", False)) or
+    not bool(getattr(lead, "radar", False)) or
+    float(getattr(lead, "modelProb", 0.0)) < 0.5 or float(v_ego) < 15.0
+  ):
+    return False
+
+  closing_speed = float(v_ego) - max(float(lead.vLead), 0.0)
+  return closing_speed >= 7.0 and 30.0 <= float(lead.dRel) <= min(120.0, 4.0 * float(v_ego))
+
+
+def get_toyota_corolla_braking_lead_cap(CP, lead, v_ego, desired_gap, accel_min):
+  if (
+    getattr(CP, "brand", "") != "toyota" or
+    str(getattr(CP, "carFingerprint", "")) != "TOYOTA_COROLLA_TSS2" or
+    lead is None or not bool(getattr(lead, "status", False)) or
+    not bool(getattr(lead, "radar", False)) or
+    float(v_ego) < 10.0 or
+    float(getattr(lead, "vLead", 0.0)) < 2.0 or
+    abs(float(getattr(lead, "yRel", 0.0))) > 1.2
+  ):
+    return None
+
+  lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
+  distance = float(getattr(lead, "dRel", float("inf")))
+  if (
+    lead_brake < 0.6 or
+    float(v_ego) - float(lead.vLead) < 0.75 or
+    distance <= 0.0 or distance > min(60.0, 3.0 * float(v_ego)) or
+    distance > float(desired_gap) + 6.0
+  ):
+    return None
+
+  return max(float(accel_min), -min(TOYOTA_COROLLA_BRAKING_LEAD_MAX_DECEL, 0.65 * lead_brake))
+
+
 def is_honda_crv_5g(CP):
  return (
    getattr(CP, "brand", "") == "honda" and
@@ -532,6 +589,11 @@ def allow_radar_standstill_gap_settle(CP):


 def get_far_follow_output_slew_rates(CP):
+  if getattr(CP, "brand", "") == "hyundai" and str(getattr(CP, "carFingerprint", "")) == "KIA_NIRO_EV":
+    return (
+      KIA_NIRO_EV_FAR_FOLLOW_BRAKE_SLEW_RATE,
+      KIA_NIRO_EV_FAR_FOLLOW_RELEASE_SLEW_RATE,
+    )
  if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD":
    return (
      HONDA_ACCORD_FAR_FOLLOW_BRAKE_SLEW_RATE,
@@ -748,9 +810,11 @@ def get_force_stop_reanchor_speed_tolerance(car_params):


 def get_force_stop_low_speed_hold(car_params):
-  """Keep a committed Santa Fe stop from releasing while it is still rolling."""
-  if str(getattr(car_params, "carFingerprint", car_params)) == "HYUNDAI_SANTA_FE_2022":
+  fingerprint = str(getattr(car_params, "carFingerprint", car_params))
+  if fingerprint == "HYUNDAI_SANTA_FE_2022":
    return HYUNDAI_SANTA_FE_2022_FORCE_STOP_LOW_SPEED_HOLD
+  if fingerprint == "FORD_MUSTANG_MACH_E_MK1":
+    return FORD_MACH_E_FORCE_STOP_LOW_SPEED_HOLD
  return None


@@ -73,3 +73,83 @@ def test_strong_turn_and_driver_input_reset_stabilizer():

  assert update_accel(stabilizer, -0.3, enabled=False) == pytest.approx(-0.3)
  assert update_accel(stabilizer, 0.3) == pytest.approx(0.3)
+
+
+def test_slow_highway_oscillation_stays_damped_between_reversals():
+  stabilizer = GenesisGV70HighwayCommandStabilizer()
+  raw, shaped, blends = [], [], []
+  for i in range(3000):
+    accel = 0.50 + 0.20 * math.sin(2.0 * math.pi * 0.28 * i * 0.01)
+    raw.append(accel)
+    shaped.append(update_accel(stabilizer, accel))
+    blends.append(stabilizer.blend)
+
+  assert min(blends[1500:]) > 0.99
+  assert np.std(shaped[1500:]) < 0.70 * np.std(raw[1500:])
+  assert np.mean(shaped[1500:]) == pytest.approx(np.mean(raw[1500:]), abs=0.015)
+
+
+def test_stabilizer_recovers_after_oscillation_ends():
+  stabilizer = GenesisGV70HighwayCommandStabilizer()
+  for i in range(1600):
+    update_accel(stabilizer, 0.50 + 0.20 * math.sin(2.0 * math.pi * 0.4 * i * 0.01))
+  assert stabilizer.blend > 0.9
+
+  for _ in range(1500):
+    shaped = update_accel(stabilizer, 0.50)
+  assert stabilizer.blend < 0.001
+  assert shaped == pytest.approx(0.50, abs=1e-6)
+
+
+@pytest.mark.parametrize('direction', [-1.0, 1.0])
+def test_real_curve_direction_change_bypasses_recovery(direction):
+  stabilizer = GenesisGV70HighwayCommandStabilizer()
+  for i in range(1600):
+    update_accel(stabilizer, direction * (0.55 + 0.20 * math.sin(2.0 * math.pi * 0.4 * i * 0.01)))
+  assert stabilizer.blend > 0.9
+
+  assert update_accel(stabilizer, -direction * 0.60) == pytest.approx(-direction * 0.60)
+  assert stabilizer.blend == 0.0
+  assert not stabilizer.reversals
+  assert stabilizer.active_until == 0.0
+  assert stabilizer.curve_direction == 0
+
+
+@pytest.mark.parametrize('direction', [-1.0, 1.0])
+def test_gradual_s_curve_does_not_delay_direction_change(direction):
+  stabilizer = GenesisGV70HighwayCommandStabilizer()
+  for i in range(1600):
+    update_accel(stabilizer, direction * (0.55 + 0.20 * math.sin(2.0 * math.pi * 0.4 * i * 0.01)))
+  assert stabilizer.blend > 0.9
+
+  raw = direction * np.linspace(0.55, -0.65, 500)
+  shaped = np.array([update_accel(stabilizer, float(accel)) for accel in raw])
+  raw_crossing = np.flatnonzero(direction * raw < 0.0)[0]
+  shaped_crossing = np.flatnonzero(direction * shaped < 0.0)[0]
+  assert shaped_crossing == raw_crossing
+  assert shaped[raw_crossing:] == pytest.approx(raw[raw_crossing:])
+
+
+def test_inactive_reset_clears_recovery_and_reengages_cleanly():
+  stabilizer = GenesisGV70HighwayCommandStabilizer()
+  for i in range(1600):
+    update_accel(stabilizer, 0.50 + 0.20 * math.sin(2.0 * math.pi * 0.4 * i * 0.01))
+  assert stabilizer.active_until > stabilizer.elapsed
+
+  assert update_accel(stabilizer, 0.60, enabled=False) == pytest.approx(0.60)
+  assert stabilizer.active_until == 0.0
+  assert stabilizer.baseline is None
+  assert update_accel(stabilizer, -0.60) == pytest.approx(-0.60)
+
+
+@pytest.mark.parametrize('speed', [40.1, 45.0, 50.0, 65.0])
+def test_recovery_respects_speed_gate_and_correction_bound(speed):
+  stabilizer = GenesisGV70HighwayCommandStabilizer()
+  speed *= CV.MPH_TO_MS
+  max_delta = 0.0
+  for i in range(1600):
+    accel = 0.3 * math.sin(2.0 * math.pi * 0.4 * i * 0.01)
+    shaped = update_accel(stabilizer, accel, speed)
+    max_delta = max(max_delta, abs(shaped-accel))
+  speed_weight = np.interp(speed, [40.0*CV.MPH_TO_MS,50.0*CV.MPH_TO_MS], [0.0,1.0])
+  assert 0.0 < max_delta <= 0.20*speed_weight+1e-6
@@ -139,18 +139,59 @@ def test_offset_is_reduced_in_narrow_lane():
  assert np.isclose(at_safe_limit, above_safe_limit)


-def test_confident_e2e_path_can_fully_break_in():
-  model = _model(left=-1.0, right=2.6, model_y=0.0, path_std=0.1)
+@pytest.mark.parametrize("direction", [-1.0, 1.0])
+def test_confident_e2e_path_retains_bounded_lane_correction(direction):
+  model = _model(left=-2.4, right=2.4, model_y=direction * 0.6, path_std=0.1)
  _, lane_authority = _converge(model, authority=0.0)
  _, e2e_authority = _converge(model, authority=1.0)
-  assert lane_authority > 0.0
-  assert abs(e2e_authority) < 1e-9
+  assert lane_authority * direction < 0.0
+  assert e2e_authority == pytest.approx(0.2 * lane_authority)
+
+
+@pytest.mark.parametrize("direction", [-1.0, 1.0])
+def test_e2e_retains_more_lane_correction_near_boundary(direction):
+  model = _model(model_y=direction * 0.8, path_std=0.1)
+  _, lane_authority = _converge(model, authority=0.0)
+  _, e2e_authority = _converge(model, authority=1.0)
+  assert e2e_authority == pytest.approx(0.5 * lane_authority)
+
+
+def test_e2e_boundary_authority_blends_continuously():
+  fractions = []
+  for clearance in np.linspace(1.55, 1.05, 101):
+    model = _model(left=-2.4, right=2.4, model_y=-2.4 + clearance)
+    lane_valid, lane_authority = LaneCenteringController._raw_correction(model, _V_EGO, 0.0, 0.0)
+    e2e_valid, e2e_authority = LaneCenteringController._raw_correction(model, _V_EGO, 0.0, 1.0)
+    assert lane_valid and e2e_valid
+    fractions.append(e2e_authority / lane_authority)
+  assert fractions[0] == pytest.approx(0.2)
+  assert fractions[-1] == pytest.approx(0.5)
+  assert np.all(np.diff(fractions) >= -1e-9)
+  assert np.max(np.diff(fractions)) < 0.004
+
+
+@pytest.mark.parametrize("line", [1, 2])
+def test_e2e_boundary_correction_requires_both_lane_lines(line):
+  model = _model(model_y=-0.8)
+  assert _update(LaneCenteringController(), model) > 0.0
+  model.laneLineProbs[line] = 0.59
+  assert _update(LaneCenteringController(), model) == 0.0
+
+
+@pytest.mark.parametrize("direction", [-1.0, 1.0])
+def test_e2e_boundary_correction_remains_capped_and_yields_to_driver(direction):
+  model = _model(model_y=direction * 2.0)
+  controller, output = _converge(model)
+  assert output * direction < 0.0
+  assert abs(output) <= 0.004 * 0.30
+  assert _update(controller, model, driver_override=True) == 0.0


 def test_uncertain_e2e_path_does_not_break_in():
  model = _model(left=-1.0, right=2.6, model_y=0.0, path_std=0.6)
+  _, lane_only = _converge(model, authority=0.0)
  _, output = _converge(model, authority=1.0)
-  assert output > 0.0
+  assert output == pytest.approx(lane_only)


 def test_e2e_authority_blends_lane_correction():
@@ -376,6 +376,30 @@ def test_hrv_far_follow_output_slew_damps_only_continuous_safe_follow():
  assert smoothed == pytest.approx(-0.5)


+def test_niro_ev_far_follow_slew_is_vehicle_specific_and_preserves_urgent_braking():
+  CP = HyundaiCarInterface.get_non_essential_params(HYUNDAI_CAR.KIA_NIRO_EV)
+  next_gen = HyundaiCarInterface.get_non_essential_params(HYUNDAI_CAR.KIA_NIRO_EV_2ND_GEN)
+  planner = LongitudinalPlanner(CP, init_v=16.0)
+  planner.lead_one = make_lead(status=True, d_rel=45.0, v_lead=15.0, model_prob=0.99, y_rel=0.0)
+  planner.lead_two = make_lead(status=False)
+
+  assert get_far_follow_output_slew_rates(CP) == pytest.approx((2.5, 1.75))
+  assert get_far_follow_output_slew_rates(next_gen) == (0.0, 0.0)
+  first = planner.get_vehicle_far_follow_slew_target(16.0, 0.0, -0.4, False, False)
+  release = planner.get_vehicle_far_follow_slew_target(16.0, first, 0.3, False, False)
+  assert first == pytest.approx(-0.4)
+  assert release == pytest.approx(first + 1.75 * planner.dt)
+
+  planner.lead_one.dRel = 18.0
+  assert planner.get_vehicle_far_follow_slew_target(16.0, release, -1.5, False, False) == pytest.approx(-1.5)
+  assert not planner.far_follow_output_slew_active
+
+  planner.lead_one.dRel = 45.0
+  planner.lead_one.vLead = 10.0
+  assert planner.get_vehicle_far_follow_slew_target(16.0, release, -1.5, False, False) == pytest.approx(-1.5)
+  assert not planner.far_follow_output_slew_active
+
+
 def test_crv_far_follow_output_slew_damps_nonurgent_lead_transition():
  v_ego = 24.0
  CP = CarInterface.get_non_essential_params(CAR.HONDA_CRV_5G)
@@ -857,6 +857,65 @@ def test_santa_fe_force_stop_holds_through_low_speed_detector_dropout():
  assert result == pytest.approx(0.0)


+@pytest.mark.parametrize("fingerprint", ("FORD_MUSTANG_MACH_E_MK1", "FORD_F_150_LIGHTNING_MK1", "OTHER_CAR"))
+def test_mach_e_force_stop_completion_is_vehicle_scoped(fingerprint):
+  planner, vcruise = make_vcruise(red_light=True, forcing_stop=True)
+  planner.model_length = 60.0
+  vcruise.force_stop_entry_speed = 12.0
+  vcruise.tracked_model_length = 4.6
+  vcruise.force_stop_distance_cap = 4.6
+  sm = make_sm(standstill=False, car_fingerprint=fingerprint)
+  sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
+  toggles = make_toggles()
+
+  update_vcruise(vcruise, sm, toggles, now=0.0, v_ego=0.7)
+  planner.starpilot_cem.stop_light_detected = False
+  update_vcruise(vcruise, sm, toggles, now=0.25, v_ego=0.7)
+  result = update_vcruise(vcruise, sm, toggles, now=0.8, v_ego=0.7)
+
+  if fingerprint == "FORD_MUSTANG_MACH_E_MK1":
+    assert get_force_stop_low_speed_hold(sm["carParams"]) == pytest.approx(1.0)
+    assert vcruise.forcing_stop
+    assert result == pytest.approx(0.0)
+    assert vcruise.tracked_model_length <= 4.6
+  else:
+    assert get_force_stop_low_speed_hold(sm["carParams"]) is None
+    assert not vcruise.forcing_stop
+    assert result == pytest.approx(20.0)
+
+
+def test_mach_e_force_stop_still_releases_green_above_final_handoff():
+  planner, vcruise = make_vcruise(red_light=True, forcing_stop=True)
+  vcruise.force_stop_entry_speed = 12.0
+  sm = make_sm(standstill=False, car_fingerprint="FORD_MUSTANG_MACH_E_MK1")
+  toggles = make_toggles()
+
+  update_vcruise(vcruise, sm, toggles, now=0.0, v_ego=3.0)
+  planner.starpilot_cem.stop_light_detected = False
+  update_vcruise(vcruise, sm, toggles, now=0.25, v_ego=3.0)
+  result = update_vcruise(vcruise, sm, toggles, now=0.8, v_ego=3.0)
+
+  assert not vcruise.forcing_stop
+  assert result == pytest.approx(20.0)
+
+
+@pytest.mark.parametrize("override", ("gas", "standstill"))
+def test_mach_e_force_stop_completion_does_not_block_departure(override):
+  planner, vcruise = make_vcruise(red_light=True, forcing_stop=True)
+  vcruise.force_stop_entry_speed = 12.0
+  sm = make_sm(standstill=False, car_fingerprint="FORD_MUSTANG_MACH_E_MK1")
+  toggles = make_toggles()
+
+  update_vcruise(vcruise, sm, toggles, now=0.0, v_ego=0.7)
+  planner.starpilot_cem.stop_light_detected = False
+  sm["carState"].gasPressed = override == "gas"
+  sm["carState"].standstill = override == "standstill"
+  result = update_vcruise(vcruise, sm, toggles, now=0.8, v_ego=0.0 if override == "standstill" else 0.7)
+
+  assert not vcruise.forcing_stop
+  assert result == pytest.approx(20.0)
+
+
 def test_force_stop_does_not_reanchor_inside_reanchor_floor():
  planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
  planner.model_length = 90.0
@@ -38,8 +38,8 @@ MACH_E_TURN_IN_LOOKAHEAD_EXTRA = 0.80
 MACH_E_LOW_SPEED_TURN_IN_LOOKAHEAD_EXTRA = 1.60
 MACH_E_LOW_SPEED_TURN_IN_START_SPEED = 2.0
 MACH_E_LOW_SPEED_TURN_IN_FULL_SPEED = 3.0
-MACH_E_LOW_SPEED_TURN_IN_MAX_SPEED = 9.0
-MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED = 12.0
+MACH_E_LOW_SPEED_TURN_IN_MAX_SPEED = 11.0
+MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED = 14.0
 MACH_E_TURN_IN_MIN_CURVATURE = 0.002
 MACH_E_TURN_IN_FULL_CURVATURE = 0.008
 MACH_E_TURN_IN_LAG_CURVATURE = 0.006
@@ -78,13 +78,18 @@ MACH_E_SHARP_DIRECTION_CHANGE_MIN_ACCEL = 1.8
 MACH_E_SHARP_DIRECTION_CHANGE_FULL_ACCEL = 2.2
 MACH_E_SHARP_DIRECTION_CHANGE_MIN_LAG_CURVATURE = -0.0005
 MACH_E_SHARP_DIRECTION_CHANGE_FULL_LAG_CURVATURE = 0.0008
-MACH_E_UNDERSTEER_ERROR_MAX = 0.006
+MACH_E_CURVATURE_ERROR_MAX = 0.006
 MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT = 0.002
 MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT = 0.004
 MACH_E_PATH_ANGLE_MAX = 0.16
 MACH_E_PATH_ANGLE_STEP = 0.055
 MACH_E_PATH_ANGLE_FADE_START_SPEED = 8.0
 MACH_E_PATH_ANGLE_MAX_SPEED = 8.8
+MACH_E_PATH_ANGLE_TRACKING_FACTOR = 0.75
+MACH_E_PATH_ANGLE_DRIVER_COOLDOWN = 0.75
+MACH_E_DRIVER_ASSIST_MIN_SPEED = 2.0
+MACH_E_DRIVER_ASSIST_MAX_SPEED = 15.0
+MACH_E_DRIVER_ASSIST_MAX_TORQUE = 3.5
 FORD_CURVATURE_LOOKAHEAD = {
  CAR.FORD_EXPLORER_MK6: 0.20,
 }
@@ -171,6 +176,7 @@ class FordLateralController:
    self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT)))
    self.curvature_last = 0.0
    self.path_angle_last = 0.0
+    self.path_angle_driver_cooldown = 0.0
    self.desired_curvature_last = 0.0
    self._frame = 0
    self._update_params()
@@ -220,30 +226,65 @@ class FordLateralController:
  def _current_curvature(CS) -> float:
    return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)

+  def _driver_assisting_curve(self, CS, desired: float) -> bool:
+    v_ego = float(CS.out.vEgoRaw)
+    if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or
+        not CS.out.steeringPressed or not MACH_E_DRIVER_ASSIST_MIN_SPEED <= v_ego < MACH_E_DRIVER_ASSIST_MAX_SPEED or
+        self._lane_change()[0] or abs(desired) < MACH_E_TURN_IN_MIN_CURVATURE or
+        desired * CS.out.steeringTorque >= 0.0 or abs(CS.out.steeringTorque) > MACH_E_DRIVER_ASSIST_MAX_TORQUE):
+      return False
+    current = self._current_curvature(CS)
+    preview = self._predicted_curvature(v_ego, self._curvature_lookahead() + MACH_E_TURN_IN_LOOKAHEAD_EXTRA)
+    return bool(desired * current > 0.0 and desired * preview > 0.0 and
+                abs(preview) >= MACH_E_TURN_IN_FULL_CURVATURE and
+                np.sign(desired) * (current - desired) <= CarControllerParams.CURVATURE_ERROR)
+
  def _curvature_error_limit(self, requested: float, desired: float, current: float, v_ego: float,
-                             steering_pressed: bool, lane_change: bool) -> float:
+                             steering_pressed: bool, lane_change: bool, predicted: float = 0.0) -> float:
    base = CarControllerParams.CURVATURE_ERROR
    if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or
-        steering_pressed or lane_change or requested * desired <= 0.0 or abs(desired) < 0.003):
+        steering_pressed or lane_change):
      return base
-    deficit = np.sign(desired) * (requested - current)
    speed_weight = float(np.interp(v_ego, [9.0, 10.0, 14.0, 16.0], [0.0, 1.0, 1.0, 0.0]))
-    deficit_weight = float(np.interp(
-      deficit, [MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT, MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT], [0.0, 1.0]))
-    return base + (MACH_E_UNDERSTEER_ERROR_MAX - base) * speed_weight * deficit_weight
+    deficit_weight = 0.0
+    planned_curve = abs(desired) >= 0.003 or (abs(requested) >= 0.003 and requested * predicted > 0.0)
+    if requested * desired > 0.0 and planned_curve:
+      deficit = np.sign(desired) * (requested - current)
+      deficit_weight = float(np.interp(
+        deficit, [MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT, MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT], [0.0, 1.0]))
+    if current * desired < 0.0:
+      predicted = self._predicted_curvature(v_ego, self._curvature_lookahead() + MACH_E_TURN_IN_LOOKAHEAD_EXTRA)
+    reversal_weight = 0.0
+    if current * predicted < 0.0 and current * (requested - current) < 0.0:
+      reversal_weight = float(np.interp(
+        abs(predicted),
+        [MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE],
+        [0.0, 1.0],
+      ))
+    return base + (MACH_E_CURVATURE_ERROR_MAX - base) * speed_weight * max(deficit_weight, reversal_weight)

-  def _path_angle_assist(self, requested: float, desired: float, applied: float, v_ego: float,
+  def _path_angle_assist(self, requested: float, desired: float, applied: float, current: float, v_ego: float,
                         steering_pressed: bool, lane_change: bool) -> float:
+    if steering_pressed:
+      self.path_angle_driver_cooldown = MACH_E_PATH_ANGLE_DRIVER_COOLDOWN
+    else:
+      self.path_angle_driver_cooldown = max(0.0, self.path_angle_driver_cooldown - STEER_DT)
    target = 0.0
    if (self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD and
-        not steering_pressed and not lane_change and 3.0 <= v_ego < MACH_E_PATH_ANGLE_MAX_SPEED and
+        not steering_pressed and self.path_angle_driver_cooldown == 0.0 and not lane_change and
+        3.0 <= v_ego < MACH_E_PATH_ANGLE_MAX_SPEED and
        requested * desired > 0.0 and requested * applied > 0.0 and
-        abs(requested) > 0.021 and abs(desired) > 0.021 and abs(applied) >= 0.0195):
+        abs(requested) > 0.0198 and abs(desired) > MACH_E_TURN_IN_FULL_CURVATURE and abs(applied) >= 0.0195 and
+        np.sign(desired) * (desired - current) > 0.002):
      max_curvature = MAX_LATERAL_ACCEL / v_ego ** 2
-      residual = max(0.0, min(abs(requested), abs(desired), max_curvature) - abs(applied))
+      residual = max(0.0, min(max(abs(requested), abs(desired)), max_curvature) - abs(applied))
+      tracking_deficit = max(0.0, np.sign(applied) * (applied - current))
+      acceleration_headroom = max(0.0, (max_curvature - abs(applied)) * v_ego)
      speed_weight = float(np.interp(
        v_ego, [MACH_E_PATH_ANGLE_FADE_START_SPEED, MACH_E_PATH_ANGLE_MAX_SPEED], [1.0, 0.0]))
-      target = float(np.sign(applied) * min(residual * v_ego * speed_weight, MACH_E_PATH_ANGLE_MAX))
+      target = float(np.sign(applied) * min(
+        (residual + MACH_E_PATH_ANGLE_TRACKING_FACTOR * tracking_deficit) * v_ego,
+        acceleration_headroom, MACH_E_PATH_ANGLE_MAX) * speed_weight)
    if target == 0.0 or target * self.path_angle_last < 0.0:
      self.path_angle_last = 0.0
    else:
@@ -414,7 +455,7 @@ class FordLateralController:
    ))
    return speed_weight * curvature_weight * preview_weight * acceleration_weight

-  def _manual_turn(self, CC, CS, desired: float) -> bool:
+  def _manual_turn(self, CC, CS, desired: float, driver_assisting: bool = False) -> bool:
    if not CC.latActive:
      self.human_turn.reset()
      self.manual_turn_latched = False
@@ -422,7 +463,7 @@ class FordLateralController:
      self.manual_turn_direction = 0.0
      return False
    detected = self.human_turn.update(
-      self.human_turn_enabled, CS.out.steeringPressed, CS.out.steeringAngleDeg)
+      self.human_turn_enabled and not driver_assisting, CS.out.steeringPressed, CS.out.steeringAngleDeg)
    if self.CP.carFingerprint not in FORD_MANUAL_TURN_LATCH_CARS:
      return detected

@@ -434,7 +475,7 @@ class FordLateralController:

    blinker_direction = float(CS.out.rightBlinker) - float(CS.out.leftBlinker)
    driver_turning_with_signal = (
-      CS.out.steeringPressed and abs(CS.out.steeringAngleDeg) >= MANUAL_TURN_ENTRY_ANGLE_DEG and
+      CS.out.steeringPressed and not driver_assisting and abs(CS.out.steeringAngleDeg) >= MANUAL_TURN_ENTRY_ANGLE_DEG and
      blinker_direction != 0.0 and not self._lane_change()[0] and
      CS.out.steeringTorque * blinker_direction < 0.0
    )
@@ -477,11 +518,17 @@ class FordLateralController:
      self.curvature_samples.clear()
      self.curvature_last = 0.0
      self.path_angle_last = 0.0
+      self.path_angle_driver_cooldown = 0.0
      self.desired_curvature_last = 0.0
      return FordLateralResult()

-    manual_turn = self._manual_turn(CC, CS, float(actuators.curvature))
+    desired = float(actuators.curvature)
+    driver_assisting = self._driver_assisting_curve(CS, desired)
+    driver_override = bool(CS.out.steeringPressed) and not driver_assisting
+    manual_turn = self._manual_turn(CC, CS, desired, driver_assisting)
    if manual_turn or CS.out.vEgoRaw < 0.1:
+      if CS.out.steeringPressed:
+        self.path_angle_driver_cooldown = MACH_E_PATH_ANGLE_DRIVER_COOLDOWN
      self.curvature_samples.clear()
      self.curvature_last = 0.0
      self.path_angle_last = 0.0
@@ -492,7 +539,6 @@ class FordLateralController:
    v_ego = float(CS.out.vEgoRaw)
    lookahead = self._curvature_lookahead()
    predicted = self._predicted_curvature(v_ego, lookahead)
-    desired = float(actuators.curvature)
    allow_opposite_preview = False
    if self.CP.carFingerprint in FORD_CONSERVATIVE_PREVIEW_CARS:
      turn_in_predicted = self._predicted_curvature(v_ego, lookahead + MACH_E_TURN_IN_LOOKAHEAD_EXTRA)
@@ -549,7 +595,7 @@ class FordLateralController:

    if v_ego > 9.0:
      error_limit = self._curvature_error_limit(
-        requested, desired, current, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0])
+        requested, desired, current, v_ego, driver_override, self._lane_change()[0], command_predicted)
      requested = float(np.clip(requested, current - error_limit, current + error_limit))
    applied = float(apply_std_steer_angle_limits(
      requested, self.curvature_last, v_ego, CS.out.steeringAngleDeg, True, FORD_CURVATURE_LIMITS))
@@ -557,7 +603,7 @@ class FordLateralController:
      max_curvature = MAX_LATERAL_ACCEL / max(v_ego, 1.0) ** 2
      applied = float(np.clip(applied, -max_curvature, max_curvature))
    path_angle = self._path_angle_assist(
-      requested, desired, applied, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0])
+      requested, desired, applied, current, v_ego, driver_override, self._lane_change()[0])

    self.curvature_samples.append(predicted)
    curvature_rate = 0.0
@@ -117,20 +117,152 @@ def test_understeer_error_preserves_other_fords(controller):
  assert controller._curvature_error_limit(0.012, 0.012, 0.004, 12.0, False, False) == 0.002


+@pytest.mark.parametrize("sign", (-1, 1))
+@pytest.mark.parametrize("speed,expected", ((9.0, 0.002), (9.5, 0.004), (10.0, 0.006),
+                                          (12.0, 0.006), (15.0, 0.004), (16.0, 0.002)))
+def test_mach_e_planned_curve_error_uses_preview_request_before_action_builds(controller, sign, speed, expected):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  assert controller._curvature_error_limit(
+    sign * 0.006, sign * 0.001, sign * 0.0005, speed, False, False, sign * 0.012) == pytest.approx(expected)
+
+
+@pytest.mark.parametrize("requested,desired,preview", ((0.006, 0.001, -0.012),
+                                                     (0.006, 0.001, 0.0),
+                                                     (0.006, -0.001, 0.012),
+                                                     (0.0029, 0.001, 0.012)))
+def test_mach_e_planned_curve_error_requires_curve_size_and_direction_agreement(controller, requested, desired, preview):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  assert controller._curvature_error_limit(requested, desired, 0.0005, 12.0, False, False, preview) == 0.002
+
+
+@pytest.mark.parametrize("fingerprint,flags,driver,lane_change", (
+  (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, True, False),
+  (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, True),
+  (CAR.FORD_MUSTANG_MACH_E_MK1, 0, False, False),
+  (CAR.FORD_EDGE_MK2, FordFlags.CANFD, False, False),
+  (CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, False, False),
+  (CAR.FORD_F_150_MK14, FordFlags.CANFD, False, False),
+))
+def test_planned_curve_error_preserves_takeover_lane_changes_and_other_fords(controller, fingerprint, flags, driver, lane_change):
+  controller.CP.carFingerprint = fingerprint
+  controller.CP.flags = flags
+  assert controller._curvature_error_limit(0.006, 0.001, 0.0005, 12.0, driver, lane_change, 0.012) == 0.002
+
+
+@pytest.mark.parametrize("sign", (-1, 1))
+def test_mach_e_medium_speed_turn_in_lead_is_not_clipped_by_small_action_curvature(controller, monkeypatch, sign):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  controller.sm["liveDelay"].lateralDelay = 0.4
+  controller.desired_curvature_last = sign * 0.0003
+  controller.curvature_last = sign * 0.003
+  monkeypatch.setattr(controller, "_predicted_curvature",
+                      lambda _v, t: sign * (0.0007 if t < 1.0 else 0.0053 if t < 1.5 else 0.011))
+  result = controller.update(SimpleNamespace(latActive=True), car_state(speed=11.8, curvature=sign * 0.00027),
+                             SimpleNamespace(curvature=sign * 0.0004))
+  assert result.active
+  assert 0.004 < sign * result.curvature <= 0.00627
+  assert result.path_angle == 0.0
+
+
+@pytest.mark.parametrize("sign", (-1, 1))
+@pytest.mark.parametrize("speed,preview,expected", (
+  (9.0, -0.004, 0.002),
+  (9.5, -0.004, 0.004),
+  (10.0, -0.004, 0.006),
+  (12.0, -0.004, 0.006),
+  (14.0, -0.004, 0.006),
+  (15.0, -0.004, 0.004),
+  (16.0, -0.004, 0.002),
+  (12.0, -0.0005, 0.002),
+  (12.0, -0.00125, 0.004),
+  (12.0, 0.004, 0.002),
+  (12.0, 0.0, 0.002),
+))
+def test_mach_e_reversal_error_releases_measured_curvature_clamp(controller, sign, speed, preview, expected):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  assert controller._curvature_error_limit(
+    sign * 0.001, sign * 0.002, sign * 0.005, speed, False, False, sign * preview) == pytest.approx(expected)
+
+
+@pytest.mark.parametrize("fingerprint,flags,driver,lane_change", (
+  (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, True, False),
+  (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, True),
+  (CAR.FORD_MUSTANG_MACH_E_MK1, 0, False, False),
+  (CAR.FORD_EDGE_MK2, FordFlags.CANFD, False, False),
+  (CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, False, False),
+  (CAR.FORD_F_150_MK14, FordFlags.CANFD, False, False),
+))
+def test_reversal_error_preserves_takeover_lane_changes_and_other_fords(controller, fingerprint, flags, driver, lane_change):
+  controller.CP.carFingerprint = fingerprint
+  controller.CP.flags = flags
+  assert controller._curvature_error_limit(0.001, 0.002, 0.005, 12.0, driver, lane_change, -0.004) == 0.002
+
+
+def test_mach_e_reversal_error_does_not_increase_old_direction_command(controller):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  assert controller._curvature_error_limit(0.008, 0.002, 0.005, 12.0, False, False, -0.004) == 0.002
+
+
+@pytest.mark.parametrize("sign", (-1, 1))
+@pytest.mark.parametrize("preview,expected", ((-0.004, 0.006), (-0.0005, 0.002), (0.004, 0.002)))
+def test_mach_e_reversal_error_requires_preview_agreement_after_desired_crosses_zero(
+    controller, monkeypatch, sign, preview, expected):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * preview)
+  assert controller._curvature_error_limit(
+    -sign * 0.00034, -sign * 0.00034, sign * 0.00462, 12.0, False, False, sign * 0.0007) == pytest.approx(expected)
+
+
+@pytest.mark.parametrize("sign", (-1, 1))
+def test_mach_e_reversal_does_not_reapply_old_direction_at_desired_zero_crossing(controller, monkeypatch, sign):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  controller.sm["liveDelay"].lateralDelay = 0.4
+  controller.desired_curvature_last = sign * 0.00155
+  controller.curvature_last = -sign * 0.00065
+  monkeypatch.setattr(controller, "_predicted_curvature",
+                      lambda _v, t: sign * 0.0007 if t < 1.0 else -sign * 0.005)
+  result = controller.update(SimpleNamespace(latActive=True), car_state(speed=12.0, curvature=sign * 0.00462),
+                             SimpleNamespace(curvature=-sign * 0.00034))
+  assert result.active
+  assert result.curvature == pytest.approx(-sign * 0.00034)
+  assert result.path_angle == 0.0
+
+
 def test_mach_e_path_angle_assist_at_curvature_limit(controller):
  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
  controller.CP.flags = FordFlags.CANFD
-  outputs = [controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False) for _ in range(3)]
+  outputs = [controller._path_angle_assist(0.04, 0.04, 0.02, 0.02, 7.5, False, False) for _ in range(3)]
  assert outputs == pytest.approx([0.055, 0.110, 0.150])
-  assert controller._path_angle_assist(0.018, 0.018, 0.018, 7.5, False, False) == 0.0
-  assert controller._path_angle_assist(-0.04, -0.04, -0.02, 7.5, False, False) == pytest.approx(-0.055)
-  assert controller._path_angle_assist(-0.04, -0.04, -0.02, 7.5, True, False) == 0.0
+  assert controller._path_angle_assist(0.018, 0.018, 0.018, 0.018, 7.5, False, False) == 0.0
+  assert controller._path_angle_assist(-0.04, -0.04, -0.02, -0.02, 7.5, False, False) == pytest.approx(-0.055)
+  assert controller._path_angle_assist(-0.04, -0.04, -0.02, -0.02, 7.5, True, False) == 0.0
+
+
+@pytest.mark.parametrize("sign", (-1, 1))
+def test_mach_e_path_angle_assist_starts_at_saturation_and_releases_after_driver(controller, sign):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  request = (sign * 0.0205, sign * 0.019, sign * 0.02, sign * 0.007, 7.0)
+  assert controller._path_angle_assist(*request, False, False) == pytest.approx(sign * 0.055)
+  assert controller._path_angle_assist(*request, True, False) == 0.0
+  for _ in range(round(0.75 / STEER_DT) - 1):
+    assert controller._path_angle_assist(*request, False, False) == 0.0
+  assert controller._path_angle_assist(*request, False, False) == pytest.approx(sign * 0.055)
+  assert controller._path_angle_assist(
+    sign * 0.0205, sign * 0.019, sign * 0.02, sign * 0.021, 7.0, False, False) == 0.0


@pytest.mark.parametrize("speed,requested,desired,applied,driver,lane_change", (
  (9.0, 0.04, 0.04, 0.02, False, False),
-  (7.5, 0.020, 0.04, 0.02, False, False),
-  (7.5, 0.04, 0.018, 0.02, False, False),
+  (7.5, 0.0197, 0.04, 0.02, False, False),
+  (7.5, 0.04, 0.007, 0.02, False, False),
  (7.5, 0.04, 0.04, 0.018, False, False),
  (7.5, 0.04, 0.04, 0.02, True, False),
  (7.5, 0.04, 0.04, 0.02, False, True),
@@ -138,18 +270,18 @@ def test_mach_e_path_angle_assist_at_curvature_limit(controller):
 def test_mach_e_path_angle_assist_is_scoped(controller, speed, requested, desired, applied, driver, lane_change):
  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
  controller.CP.flags = FordFlags.CANFD
-  assert controller._path_angle_assist(requested, desired, applied, speed, driver, lane_change) == 0.0
+  assert controller._path_angle_assist(requested, desired, applied, 0.01, speed, driver, lane_change) == 0.0


 def test_path_angle_assist_preserves_other_fords(controller):
  controller.CP.flags = FordFlags.CANFD
-  assert controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False) == 0.0
+  assert controller._path_angle_assist(0.04, 0.04, 0.02, 0.01, 7.5, False, False) == 0.0


 def test_mach_e_path_angle_assist_is_encoded_with_curvature(controller):
  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
  controller.CP.flags = FordFlags.CANFD
-  assist = controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False)
+  assist = controller._path_angle_assist(0.04, 0.04, 0.02, 0.02, 7.5, False, False)
  packer = CANPacker("ford_lincoln_base_pt")
  can_bus = CanBus(SimpleNamespace(flags=FordFlags.CANFD, safetyConfigs=[SimpleNamespace()]))
  _, data, _ = fordcan.create_lat_ctl2_msg(packer, can_bus, 1, 2, 1, -0.02, 0.0, 0, -assist)
@@ -157,6 +289,135 @@ def test_mach_e_path_angle_assist_is_encoded_with_curvature(controller):
  assert encoded_angle == pytest.approx(-assist)


+@pytest.mark.parametrize("sign", (-1, 1))
+@pytest.mark.parametrize("speed,expected", ((1.9, False), (2.0, True), (3.0, True), (7.0, True),
+                                          (11.0, True), (14.9, True), (15.0, False)))
+def test_mach_e_driver_curve_assistance_scope(controller, monkeypatch, sign, speed, expected):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.02)
+  state = car_state(speed=speed, curvature=sign * 0.003, steering_pressed=True,
+                    steering_torque=-sign * 2.0)
+  assert controller._driver_assisting_curve(state, sign * 0.004) is expected
+
+
+@pytest.mark.parametrize("driver,torque,current,desired,preview,lane_change", (
+  (False, -2.0, 0.008, 0.020, 0.025, False),
+  (True, 2.0, 0.008, 0.020, 0.025, False),
+  (True, -3.6, 0.008, 0.020, 0.025, False),
+  (True, -2.0, -0.008, 0.020, 0.025, False),
+  (True, -2.0, 0.023, 0.020, 0.025, False),
+  (True, -2.0, 0.001, 0.0019, 0.025, False),
+  (True, -2.0, 0.008, 0.020, -0.025, False),
+  (True, -2.0, 0.008, 0.020, 0.007, False),
+  (True, -2.0, 0.008, 0.020, 0.025, True),
+))
+def test_mach_e_driver_curve_assistance_requires_path_agreement(
+    controller, monkeypatch, driver, torque, current, desired, preview, lane_change):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: preview)
+  monkeypatch.setattr(controller, "_lane_change", lambda: (lane_change, 0))
+  state = car_state(speed=7.0, curvature=current, steering_pressed=driver, steering_torque=torque)
+  assert not controller._driver_assisting_curve(state, desired)
+
+
+@pytest.mark.parametrize("fingerprint,flags", ((CAR.FORD_EDGE_MK2, FordFlags.CANFD),
+                                            (CAR.FORD_F_150_MK14, FordFlags.CANFD),
+                                            (CAR.FORD_MUSTANG_MACH_E_MK1, 0)))
+def test_driver_curve_assistance_preserves_other_fords(controller, monkeypatch, fingerprint, flags):
+  controller.CP.carFingerprint = fingerprint
+  controller.CP.flags = flags
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: 0.025)
+  state = car_state(speed=7.0, curvature=0.008, steering_pressed=True, steering_torque=-2.0)
+  assert not controller._driver_assisting_curve(state, 0.020)
+
+
+@pytest.mark.parametrize("sign", (-1, 1))
+def test_mach_e_driver_assistance_handoff_and_takeover(controller, monkeypatch, sign):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  controller.curvature_last = sign * 0.020
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.030)
+  CC = SimpleNamespace(latActive=True)
+  actuators = SimpleNamespace(curvature=sign * 0.022)
+  helping = car_state(speed=7.0, curvature=sign * 0.008, steering_pressed=True,
+                      steering_angle=-sign * 50.0, steering_torque=-sign * 2.0,
+                      left_blinker=sign < 0, right_blinker=sign > 0)
+  for _ in range(round(3.5 / STEER_DT)):
+    result = controller.update(CC, helping, actuators)
+    assert result.active
+    assert result.curvature == pytest.approx(sign * 0.020)
+    assert sign * result.path_angle > 0.0
+    assert not controller.manual_turn_latched
+
+  helping.out.steeringPressed = False
+  helping.out.steeringTorque = 0.0
+  result = controller.update(CC, helping, actuators)
+  assert result.active
+  assert sign * result.path_angle > 0.0
+  assert controller.path_angle_driver_cooldown == 0.0
+
+  helping.out.steeringPressed = True
+  helping.out.steeringTorque = sign * 2.0
+  result = controller.update(CC, helping, actuators)
+  assert result.path_angle == 0.0
+  assert controller.path_angle_driver_cooldown > 0.0
+
+  helping.out.steeringTorque = -sign * 3.6
+  result = controller.update(CC, helping, actuators)
+  assert not result.active
+  assert result.curvature == result.path_angle == 0.0
+  assert controller.manual_turn_latched
+  helping.out.steeringTorque = -sign * 2.0
+  assert not controller.update(CC, helping, actuators).active
+
+  controller.update(SimpleNamespace(latActive=False), helping, actuators)
+  helping.out.steeringTorque = -sign * 2.0
+  helping.out.yawRate = -sign * 0.025 * helping.out.vEgoRaw
+  result = controller.update(CC, helping, actuators)
+  assert not result.active
+  assert result.curvature == result.path_angle == 0.0
+  assert controller.manual_turn_latched
+
+  controller.update(SimpleNamespace(latActive=False), helping, actuators)
+  assert not controller.manual_turn_latched
+  assert controller.path_angle_driver_cooldown == 0.0
+
+
+@pytest.mark.parametrize("sign", (-1, 1))
+def test_mach_e_driver_help_at_early_curve_entry(controller, monkeypatch, sign):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.030)
+  state = car_state(speed=2.7, curvature=sign * 0.003, steering_pressed=True,
+                    steering_torque=-sign * 2.0, steering_angle=-sign * 15.0,
+                    left_blinker=sign < 0, right_blinker=sign > 0)
+  CC = SimpleNamespace(latActive=True)
+  assert controller.update(CC, state, SimpleNamespace(curvature=sign * 0.004)).active
+  state.out.vEgoRaw = 3.5
+  state.out.yawRate = -sign * 0.004 * state.out.vEgoRaw
+  for _ in range(8):
+    result = controller.update(CC, state, SimpleNamespace(curvature=sign * 0.022))
+    assert result.active
+  assert result.curvature == pytest.approx(sign * 0.020)
+  assert sign * result.path_angle > 0.0
+
+
+@pytest.mark.parametrize("sign", (-1, 1))
+def test_mach_e_driver_help_preserves_curvature_error_authority(controller, monkeypatch, sign):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  controller.curvature_last = sign * 0.010
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.018)
+  state = car_state(speed=11.0, curvature=sign * 0.006, steering_pressed=True,
+                    steering_torque=-sign * 2.0, steering_angle=-sign * 20.0)
+  result = controller.update(SimpleNamespace(latActive=True), state, SimpleNamespace(curvature=sign * 0.012))
+  assert result.active
+  assert sign * result.curvature > 0.010
+  assert result.path_angle == 0.0
+
+
@pytest.mark.parametrize("sign", (-1, 1))
 def test_mach_e_unwind_anticipates_opening_curve_before_current_request_is_met(controller, monkeypatch, sign):
  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
@@ -372,8 +633,11 @@ def test_mach_e_turn_in_preview_is_not_carried_into_unwind(controller):
  (3.0, 1.60),
  (8.0, 1.60),
  (9.0, 1.60),
-  (10.5, 1.20),
-  (12.0, 0.80),
+  (10.5, 1.60),
+  (11.0, 1.60),
+  (12.0, 4.0 / 3.0),
+  (13.0, 16.0 / 15.0),
+  (14.0, 0.80),
  (15.0, 0.80),
 ))
 def test_mach_e_turn_in_lookahead_extra_fades_by_speed(controller, speed, expected):
@@ -735,7 +999,7 @@ def test_mach_e_extended_direction_horizon_does_not_replace_turn_in_preview(cont
  blend_inputs = []

  def predicted_curvature(_v_ego, lookahead):
-    return {0.4: 0.006, 1.2: 0.010, 1.6: 0.004, 2.8: -0.002}[round(lookahead, 1)]
+    return {0.4: 0.006, 1.2: 0.010, 2.0: 0.004, 2.8: -0.002}[round(lookahead, 1)]

  monkeypatch.setattr(controller, "_predicted_curvature", predicted_curvature)
  monkeypatch.setattr(
@@ -525,9 +525,6 @@ class StarPilotVCruise:
      v_ego <= force_stop_low_speed_hold and
      v_ego < self.force_stop_entry_speed - 0.25
    )
-    # The Santa Fe's model stop signal can blink off after the car has already
-    # committed to the stop. Do not turn that late dropout into a throttle
-    # release while the vehicle is still rolling through the sign.
    light_stop_cleared &= not low_speed_stop_commit
    if light_stop_cleared:
      if self.force_stop_light_clear_since is None:
@@ -39,6 +39,13 @@ def test_galaxy_does_not_assign_a_regional_label_to_ambiguous_ev6_fingerprint():
  assert catalog["model_to_label"]["KIA_EV6"] is None


+def test_galaxy_lists_2026_k4_under_existing_non_hda2_platform():
+  kia_models = the_galaxy._extract_fingerprint_models_for_make("kia")
+  assert {"value": "KIA_K4_2025", "label": "Kia K4 (without HDA II) 2025-26"} in kia_models
+  assert {"value": "KIA_K4_2025", "label": "Kia K4 (with HDA II) 2025"} in kia_models
+  assert {"value": "KIA_K4_2025", "label": "Kia K4 (with HDA II) 2025-26"} not in kia_models
+
+
 def test_manual_fingerprint_api_keeps_the_saved_value_and_label_consistent(monkeypatch):
  client, params = _params_client(monkeypatch, {}, "pc")
  monkeypatch.setattr(api_server, "_get_param_type_info", lambda: ({"CarModel"}, {"CarModel": str}))
Author	SHA1	Message	Date
firestar5683	86599a27bc	Flood Warning	2026-10-01 14:11:28 -05:00
Dom	d9f58acbb7	Merge pull request #194 from N30-PH/starpilot-honda-city-2025-firmware Honda: add observed 2025 City firmware versions	2026-10-01 12:43:20 -05:00
firestar5683	42d4a5b207	Jalisco's	2026-09-30 17:16:41 -05:00
N30	b22e2d678a	Honda City: document Brazilian model years 2023-25 Follow sunnypilot/opendbc#487 and commaai/opendbc#3812. Only update the existing documentation label; vehicle parameters are unchanged.	2026-09-30 16:13:34 -03:00
N30	26433e078e	Honda: add observed 2025 City firmware versions Add six firmware versions from the owner's recorded City EXL 2025 inventory. Preserve existing firmware and control settings. Credit baninfelipe and sunnypilot/opendbc#487; related upstream contribution commaai/opendbc#3812.	2026-09-30 16:13:21 -03:00
firestar5683	bf1b916d50	Aldi	2026-09-29 20:22:20 -05:00