From 8e1caf8fe47e49553cd5901bc3e30abb5862b305 Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Fri, 14 Aug 2026 09:53:06 -0500
Subject: [PATCH] =?UTF-8?q?Ich=20bin=20m=C3=BCde.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 opendbc_repo/opendbc/car/disable_ecu.py       | 11 ++-
 .../opendbc/car/nissan/carcontroller.py       |  2 +-
 opendbc_repo/opendbc/car/nissan/interface.py  | 11 +--
 .../opendbc/car/nissan/tests/test_nissan.py   |  6 +-
 .../opendbc/car/subaru/carcontroller.py       | 82 ++++++++++++++++++-
 .../opendbc/car/subaru/tests/test_subaru.py   | 71 +++++++++++++++-
 opendbc_repo/opendbc/safety/modes/nissan.h    |  4 +-
 .../opendbc/safety/tests/test_nissan.py       |  6 +-
 selfdrive/controls/lib/latcontrol_pid.py      | 13 ++-
 selfdrive/controls/lib/latcontrol_torque.py   | 11 +++
 .../controls/lib/latcontrol_vehicle_tunes.py  | 75 ++++++++++++++++-
 selfdrive/controls/lib/longcontrol.py         |  6 ++
 .../controls/lib/longcontrol_vehicle_tunes.py | 29 +++++++
 .../controls/lib/longitudinal_planner.py      |  4 +-
 .../lib/longitudinal_vehicle_tunes.py         |  6 +-
 selfdrive/controls/tests/test_latcontrol.py   | 52 ++++++++++++
 selfdrive/controls/tests/test_longcontrol.py  | 25 ++++++
 .../tests/test_longitudinal_planner.py        | 12 +--
 selfdrive/modeld/modeld.py                    | 35 +++++---
 selfdrive/modeld/tests/test_model_fallback.py | 41 ++++++++++
 20 files changed, 456 insertions(+), 46 deletions(-)
 create mode 100644 selfdrive/modeld/tests/test_model_fallback.py

diff --git a/opendbc_repo/opendbc/car/disable_ecu.py b/opendbc_repo/opendbc/car/disable_ecu.py
index 26c4d9580..477ecc715 100644
--- a/opendbc_repo/opendbc/car/disable_ecu.py
+++ b/opendbc_repo/opendbc/car/disable_ecu.py
@@ -24,7 +24,7 @@ def ecu_log(msg):
 
 
 def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, reset=False,
-                require_response=False, diag_request=EXT_DIAG_REQUEST, diag_response=EXT_DIAG_RESPONSE):
+                require_response=False, diag_request=EXT_DIAG_REQUEST, diag_response=EXT_DIAG_RESPONSE, response_offset=0x8):
   """Silence an ECU by disabling sending and receiving messages using UDS 0x28.
   The ECU will stay silent as long as openpilot keeps sending Tester Present.
   Set require_response for takeovers that must fail closed unless the ECU confirms communication control.
@@ -36,7 +36,8 @@ def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_r
   if reset:
     try:
       ecu_log("sending ECU reset before communication control...")
-      reset_query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [RESET_REQUEST], [RESET_RESPONSE])
+      reset_query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [RESET_REQUEST], [RESET_RESPONSE],
+                                       response_offset=response_offset)
       reset_query.get_data(timeout=timeout)
       time.sleep(0.2)
     except Exception as e:
@@ -47,7 +48,8 @@ def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_r
     try:
       # Enter extended diagnostic session
       ecu_log(f"attempt {i+1}/{retry}: diag session...")
-      query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [diag_request], [diag_response])
+      query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [diag_request], [diag_response],
+                                 response_offset=response_offset)
 
       for _, _ in query.get_data(timeout).items():
         ecu_log("diag session OK")
@@ -57,7 +59,8 @@ def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_r
 
         # Send CC command and log the response
         ecu_log("sending CC...")
-        cc_query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [com_cont_req], [b''])
+        cc_query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [com_cont_req], [b''],
+                                      response_offset=response_offset)
         cc_response = cc_query.get_data(timeout)
 
         # Log what we got back
diff --git a/opendbc_repo/opendbc/car/nissan/carcontroller.py b/opendbc_repo/opendbc/car/nissan/carcontroller.py
index 493ed41bc..1480eaf84 100644
--- a/opendbc_repo/opendbc/car/nissan/carcontroller.py
+++ b/opendbc_repo/opendbc/car/nissan/carcontroller.py
@@ -55,7 +55,7 @@ class CarController(CarControllerBase):
                                                       CC.longActive and brake_pressure > 0, brake_mode))
 
       if self.frame % 100 == 0:
-        can_sends.append(make_tester_present_msg(0x707, 1, suppress_response=True))
+        can_sends.append(make_tester_present_msg(0x707, 0, suppress_response=True))
 
     ### STEER ###
     steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
diff --git a/opendbc_repo/opendbc/car/nissan/interface.py b/opendbc_repo/opendbc/car/nissan/interface.py
index 688449505..7b0505fd6 100644
--- a/opendbc_repo/opendbc/car/nissan/interface.py
+++ b/opendbc_repo/opendbc/car/nissan/interface.py
@@ -4,12 +4,12 @@ from opendbc.car.interfaces import CarInterfaceBase
 from opendbc.car.nissan.carcontroller import CarController
 from opendbc.car.nissan.carstate import CarState
 from opendbc.car.nissan.values import CAR, CarControllerParams, NissanSafetyFlags, \
-                                       NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_DIAGNOSTIC_RESPONSE_KWP
+                                       NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_RX_OFFSET
 
 
 LEAF_LONGITUDINAL_CARS = (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC)
 LEAF_ADAS_ECU_ADDR = 0x707
-LEAF_ADAS_ECU_BUS = 1
+LEAF_ADAS_ECU_BUS = 0
 
 
 class CarInterface(CarInterfaceBase):
@@ -64,13 +64,14 @@ class CarInterface(CarInterfaceBase):
                                    uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX,
                                    uds.MESSAGE_TYPE.NORMAL])
     ecu_disabled = disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR,
-                               com_cont_req=communication_control, require_response=True)
+                               com_cont_req=communication_control, require_response=True, response_offset=NISSAN_RX_OFFSET)
     if not ecu_disabled:
       # Nissan firmware queries use the KWP-style default session. Try it after
       # standard UDS extended-session control, but still require a positive 0x68 response.
       ecu_disabled = disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR,
                                  com_cont_req=communication_control, require_response=True,
-                                 diag_request=NISSAN_DIAGNOSTIC_REQUEST_KWP, diag_response=NISSAN_DIAGNOSTIC_RESPONSE_KWP)
+                                 diag_request=NISSAN_DIAGNOSTIC_REQUEST_KWP, diag_response=NISSAN_DIAGNOSTIC_RESPONSE_KWP,
+                                 response_offset=NISSAN_RX_OFFSET)
     params.put_bool("EcuDisableFailed", not ecu_disabled)
     if ecu_disabled:
       ecu_log("Nissan Leaf ADAS TX disabled; experimental longitudinal control enabled")
@@ -89,4 +90,4 @@ class CarInterface(CarInterfaceBase):
                                    0x80 | uds.CONTROL_TYPE.ENABLE_RX_ENABLE_TX,
                                    uds.MESSAGE_TYPE.NORMAL])
     disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR,
-                com_cont_req=communication_control)
+                com_cont_req=communication_control, response_offset=NISSAN_RX_OFFSET)
diff --git a/opendbc_repo/opendbc/car/nissan/tests/test_nissan.py b/opendbc_repo/opendbc/car/nissan/tests/test_nissan.py
index 56cf442aa..bd7ace556 100644
--- a/opendbc_repo/opendbc/car/nissan/tests/test_nissan.py
+++ b/opendbc_repo/opendbc/car/nissan/tests/test_nissan.py
@@ -65,7 +65,8 @@ def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive():
   assert can_sends[0x2B0][1].hex() == "ff6090ac5b000e03"
   assert can_sends[0x1C3][1].hex() == "000000006400ff27"
   assert can_sends[0x707][1].hex() == "023e800000000000"
-  assert all(can_sends[addr][2] == 1 for addr in (0x2B0, 0x1C3, 0x707))
+  assert all(can_sends[addr][2] == 1 for addr in (0x2B0, 0x1C3))
+  assert can_sends[0x707][2] == 0
 
 
 def test_alpha_long_controller_clamps_to_panda_accel_limit():
@@ -123,7 +124,8 @@ def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled):
 
   assert len(calls) == (1 if ecu_disabled else 2)
   assert calls[0]["addr"] == 0x707
-  assert calls[0]["bus"] == 1
+  assert calls[0]["bus"] == 0
+  assert calls[0]["response_offset"] == 0x20
   assert calls[0]["require_response"] is True
   assert calls[0]["com_cont_req"] == bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL,
                                              uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX,
diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py
index 9f1e2794f..50fdd1257 100644
--- a/opendbc_repo/opendbc/car/subaru/carcontroller.py
+++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py
@@ -16,6 +16,12 @@ _SNG_ACC_MIN_DIST = 3
 _SNG_ACC_MAX_DIST = 4.5
 _LEGACY_2025_MADS_MIN_SPEED = 0.44704
 _LEGACY_2025_MADS_MAX_STEER_ANGLE = 120.0
+_LEGACY_2025_OVERRIDE_HOLD_FRAMES = 10
+_LEGACY_2025_REENGAGE_SETTLE_FRAMES = 8
+_LEGACY_2025_REENGAGE_MAX_STEER_RATE = 2.0
+_LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA = 1.0
+_LEGACY_2025_RECLAIM_FRAMES = 36
+_LEGACY_2025_RECLAIM_EXPONENT = 2.5
 _ANGLE_REENGAGE_MAX_STEER_RATE = 3.0
 _ANGLE_REENGAGE_SETTLE_FRAMES = 2
 
@@ -33,6 +39,12 @@ class CarController(CarControllerBase):
     self.driver_override = False
     self.angle_reengage_settle_frames = 0
     self.legacy_2025_lkas_active = False
+    self.legacy_2025_handoff_active = False
+    self.legacy_2025_override_hold_frames = 0
+    self.legacy_2025_reengage_settle_frames = 0
+    self.legacy_2025_reengage_reference_angle = 0.0
+    self.legacy_2025_reclaim_frames = 0
+    self.legacy_2025_reclaim_start_angle = 0.0
 
     self.cruise_button_prev = 0
     self.steer_rate_counter = 0
@@ -50,6 +62,69 @@ class CarController(CarControllerBase):
     self.epb_resume_frames_remaining = -1
     self.last_standstill_frame = 0
 
+  def _reset_legacy_2025_handoff(self):
+    self.legacy_2025_handoff_active = False
+    self.legacy_2025_override_hold_frames = 0
+    self.legacy_2025_reengage_settle_frames = 0
+    self.legacy_2025_reengage_reference_angle = 0.0
+    self.legacy_2025_reclaim_frames = 0
+    self.legacy_2025_reclaim_start_angle = 0.0
+
+  def _legacy_2025_manual_handoff(self, CS, lkas_available):
+    if not lkas_available:
+      self._reset_legacy_2025_handoff()
+      return False
+
+    if CS.out.steeringPressed:
+      self.legacy_2025_handoff_active = True
+      self.legacy_2025_override_hold_frames = _LEGACY_2025_OVERRIDE_HOLD_FRAMES
+      self.legacy_2025_reengage_settle_frames = 0
+      self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
+      self.legacy_2025_reclaim_frames = 0
+      return True
+
+    if not self.legacy_2025_handoff_active and not self.legacy_2025_lkas_active and \
+       abs(CS.out.steeringRateDeg) > _LEGACY_2025_REENGAGE_MAX_STEER_RATE:
+      self.legacy_2025_handoff_active = True
+      self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
+
+    if not self.legacy_2025_handoff_active:
+      return False
+
+    if self.legacy_2025_override_hold_frames > 0:
+      self.legacy_2025_override_hold_frames -= 1
+      if self.legacy_2025_override_hold_frames == 0:
+        self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
+      return True
+
+    wheel_stable = abs(CS.out.steeringRateDeg) <= _LEGACY_2025_REENGAGE_MAX_STEER_RATE and \
+      abs(CS.out.steeringAngleDeg - self.legacy_2025_reengage_reference_angle) <= _LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA
+    if wheel_stable:
+      self.legacy_2025_reengage_settle_frames += 1
+    else:
+      self.legacy_2025_reengage_settle_frames = 0
+      self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
+
+    if self.legacy_2025_reengage_settle_frames < _LEGACY_2025_REENGAGE_SETTLE_FRAMES:
+      return True
+
+    self.legacy_2025_handoff_active = False
+    self.legacy_2025_reengage_settle_frames = 0
+    self.legacy_2025_reclaim_frames = _LEGACY_2025_RECLAIM_FRAMES
+    self.legacy_2025_reclaim_start_angle = CS.out.steeringAngleDeg
+    return True
+
+  def _legacy_2025_reclaim_target(self, target_angle):
+    if self.legacy_2025_reclaim_frames <= 0:
+      return target_angle
+
+    progress = (_LEGACY_2025_RECLAIM_FRAMES - self.legacy_2025_reclaim_frames + 1) / _LEGACY_2025_RECLAIM_FRAMES
+    eased_progress = progress ** _LEGACY_2025_RECLAIM_EXPONENT
+    target_angle = self.legacy_2025_reclaim_start_angle + eased_progress * \
+      (target_angle - self.legacy_2025_reclaim_start_angle)
+    self.legacy_2025_reclaim_frames -= 1
+    return target_angle
+
   def lateral_angle(self, CC, CS):
     if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
       mads_only = CC.latActive and not CC.enabled
@@ -58,16 +133,15 @@ class CarController(CarControllerBase):
       lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
         CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
 
-      manual_handoff = CS.out.steeringPressed or (
-        not self.legacy_2025_lkas_active and abs(CS.out.steeringRateDeg) > _ANGLE_REENGAGE_MAX_STEER_RATE
-      )
+      manual_handoff = self._legacy_2025_manual_handoff(CS, lkas_available)
       lkas_active = lkas_available and not manual_handoff
 
       if lkas_active and not self.legacy_2025_lkas_active:
         self.apply_steer_last = CS.out.steeringAngleDeg
 
+      steer_target = self._legacy_2025_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
       apply_steer = apply_std_steer_angle_limits(
-        CC.actuators.steeringAngleDeg,
+        steer_target,
         self.apply_steer_last,
         CS.out.vEgoRaw,
         CS.out.steeringAngleDeg,
diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py
index c2f0a15d3..072941102 100644
--- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py
+++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py
@@ -258,11 +258,76 @@ def test_legacy_2025_waits_for_manual_steering_to_settle_before_reengaging():
   assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
   assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
 
-  CS.out.steeringRateDeg = 2.0
+  for i in range(9):
+    CS.out.steeringAngleDeg += 0.5
+    CS.out.steeringRateDeg = 20.0
+    msg = controller.lateral_angle(CC, CS)
+    parser.update([(3 + i, [msg])])
+    assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
+    assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
+
+  for i in range(6):
+    if i % 2:
+      CS.out.steeringAngleDeg += 0.5
+    CS.out.steeringRateDeg = 0.0 if i % 2 == 0 else 20.0
+    msg = controller.lateral_angle(CC, CS)
+    parser.update([(12 + i, [msg])])
+    assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
+    assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
+
+  CS.out.steeringRateDeg = 0.0
+  for i in range(8):
+    msg = controller.lateral_angle(CC, CS)
+    parser.update([(18 + i, [msg])])
+    assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
+
+  measured_angle = CS.out.steeringAngleDeg
   msg = controller.lateral_angle(CC, CS)
-  parser.update([(3, [msg])])
+  parser.update([(26, [msg])])
   assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
-  assert abs(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] - CS.out.steeringAngleDeg) < 1.0
+  assert abs(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] - measured_angle) < 0.1
+
+
+def test_legacy_2025_manual_handoff_reclaim_is_gradual():
+  CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
+  controller = CarController({}, CP)
+  CC = SimpleNamespace(
+    enabled=False,
+    latActive=True,
+    actuators=SimpleNamespace(steeringAngleDeg=-20.0),
+  )
+  CS = SimpleNamespace(out=SimpleNamespace(
+    vEgoRaw=3.7,
+    steeringAngleDeg=2.5,
+    steeringRateDeg=-45.0,
+    steeringPressed=True,
+    gearShifter=structs.CarState.GearShifter.drive,
+    standstill=False,
+  ))
+  parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
+
+  msg = controller.lateral_angle(CC, CS)
+  parser.update([(1, [msg])])
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
+
+  CS.out.steeringPressed = False
+  CS.out.steeringRateDeg = 0.0
+  for i in range(19):
+    msg = controller.lateral_angle(CC, CS)
+    parser.update([(2 + i, [msg])])
+
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
+  first_reclaim_angle = parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"]
+  assert first_reclaim_angle == pytest.approx(CS.out.steeringAngleDeg, abs=0.1)
+
+  reclaim_angles = []
+  for i in range(6):
+    msg = controller.lateral_angle(CC, CS)
+    parser.update([(20 + i, [msg])])
+    reclaim_angles.append(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"])
+
+  assert all(reclaim_angles[i] >= reclaim_angles[i + 1] for i in range(len(reclaim_angles) - 1))
+  assert reclaim_angles[-1] > CC.actuators.steeringAngleDeg
 
 
 def test_ascent_2023_uses_gen2_angle_bus_layout():
diff --git a/opendbc_repo/opendbc/safety/modes/nissan.h b/opendbc_repo/opendbc/safety/modes/nissan.h
index c73a94404..05314f0cc 100644
--- a/opendbc_repo/opendbc/safety/modes/nissan.h
+++ b/opendbc_repo/opendbc/safety/modes/nissan.h
@@ -173,7 +173,7 @@ static bool nissan_tx_hook(const CANPacket_t *msg) {
     }
   }
 
-  if (nissan_longitudinal && (msg->addr == 0x707U) && (msg->bus == 1U)) {
+  if (nissan_longitudinal && (msg->addr == 0x707U) && (msg->bus == 0U)) {
     violation |= (msg->data[0] != 0x02U) || (msg->data[1] != 0x3EU) || (msg->data[2] != 0x80U);
     for (int i = 3; i < 8; i++) {
       violation |= msg->data[i] != 0U;
@@ -207,7 +207,7 @@ static safety_config nissan_init(uint16_t param) {
     {0x280, 2, 8, .check_relay = true},   // CANCEL_MSG (Leaf)
     {0x2b0, 1, 8, .check_relay = true},   // Leaf propulsion/regen request
     {0x1c3, 1, 8, .check_relay = true},   // Leaf friction-brake request
-    {0x707, 1, 8, .check_relay = false},  // Leaf ADAS ECU tester present
+    {0x707, 0, 8, .check_relay = false},  // Leaf ADAS ECU tester present
   };
 
   // Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model.
diff --git a/opendbc_repo/opendbc/safety/tests/test_nissan.py b/opendbc_repo/opendbc/safety/tests/test_nissan.py
index 1251dcad9..2453d5b92 100755
--- a/opendbc_repo/opendbc/safety/tests/test_nissan.py
+++ b/opendbc_repo/opendbc/safety/tests/test_nissan.py
@@ -145,7 +145,7 @@ class TestNissanLeafSafety(TestNissanSafety):
 
 class TestNissanLeafLongSafety(TestNissanLeafSafety):
 
-  TX_MSGS = [*TestNissanLeafSafety.TX_MSGS, [0x2B0, 1], [0x1C3, 1], [0x707, 1]]
+  TX_MSGS = [*TestNissanLeafSafety.TX_MSGS, [0x2B0, 1], [0x1C3, 1], [0x707, 0]]
   RELAY_MALFUNCTION_ADDRS = {0: (0x169, 0x2B1, 0x4CC), 1: (0x2B0, 0x1C3), 2: (0x280,)}
   FWD_BLACKLISTED_ADDRS = {0: [0x280], 2: [0x169, 0x2B1, 0x4CC]}
 
@@ -261,13 +261,13 @@ class TestNissanLeafLongSafety(TestNissanLeafSafety):
     self.assertTrue(self._tx(self._brake_msg(0, active=False, brake_mode=False)))
 
   def test_tester_present(self):
-    tester_present = make_tester_present_msg(0x707, 1, suppress_response=True)
+    tester_present = make_tester_present_msg(0x707, 0, suppress_response=True)
     self.assertTrue(self._tx(self._make_msg(tester_present)))
 
     for index in range(8):
       dat = bytearray(tester_present.dat)
       dat[index] ^= 0x1
-      self.assertFalse(self._tx(common.make_msg(1, 0x707, 8, dat)), index)
+      self.assertFalse(self._tx(common.make_msg(0, 0x707, 8, dat)), index)
 
 
 if __name__ == "__main__":
diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py
index 5f674b2c7..73c37684d 100644
--- a/selfdrive/controls/lib/latcontrol_pid.py
+++ b/selfdrive/controls/lib/latcontrol_pid.py
@@ -5,7 +5,12 @@ from opendbc.car.honda.carcontroller import get_civic_bosch_modified_steering_pr
 from opendbc.car.honda.values import CAR as HONDA, HondaFlags
 from openpilot.starpilot.common.testing_grounds import testing_ground
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
-from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import SUBARU_IMPREZA_CARS, get_subaru_impreza_pid_output_scale
+from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
+  RAV4_TSS2_CARS,
+  SUBARU_IMPREZA_CARS,
+  get_rav4_tss2_pid_output,
+  get_subaru_impreza_pid_output_scale,
+)
 from openpilot.common.pid import PIDController
 
 HONDA_PID_GAIN_SCALE_MIN = 0.1
@@ -99,6 +104,7 @@ class LatControlPID(LatControl):
     self.honda_lateral_pid_ki_scale = 1.0
     self.is_civic_bosch_modified = CP.carFingerprint == HONDA.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED)
     self.is_subaru_impreza = CP.carFingerprint in SUBARU_IMPREZA_CARS
+    self.is_rav4_tss2 = CP.carFingerprint in RAV4_TSS2_CARS
     self.prev_angle_steers_des_no_offset = 0.0
     self.modified_civic_steering_pressed_filter_s = 0.0
     self.modified_civic_steering_pressed_prev = False
@@ -165,6 +171,11 @@ class LatControlPID(LatControl):
         output_torque = raw_output_torque * get_subaru_impreza_pid_output_scale(error)
         output_torque = float(max(min(output_torque, self.steer_max), -self.steer_max))
 
+      if self.is_rav4_tss2:
+        output_torque = get_rav4_tss2_pid_output(output_torque, self.prev_output_torque,
+                                                angle_steers_des_no_offset, CS.vEgo)
+        output_torque = float(max(min(output_torque, self.steer_max), -self.steer_max))
+
       if self.is_civic_bosch_modified and civic_bosch_modified_lateral_testing_ground_active():
         desired_angle_delta = angle_steers_des_no_offset - self.prev_angle_steers_des_no_offset
         output_torque *= get_civic_bosch_modified_pid_output_scale(angle_steers_des_no_offset, desired_angle_delta, CS.vEgo)
diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py
index b9965d4a1..770c0ce3a 100644
--- a/selfdrive/controls/lib/latcontrol_torque.py
+++ b/selfdrive/controls/lib/latcontrol_torque.py
@@ -469,6 +469,10 @@ class LatControlTorque(LatControl):
         ff *= get_genesis_gv70_unwind_ff_scale(
           setpoint, measurement, desired_lateral_jerk, CS.vEgo,
         )
+      if self.is_genesis_g70:
+        ff *= get_genesis_g70_unwind_ff_scale(
+          setpoint, measurement, desired_lateral_jerk, CS.vEgo,
+        )
       if ioniq_6_active:
         vehicle_friction_jerk_deadzone = (
           IONIQ_6_2025_FRICTION_JERK_DEADZONE if self.is_ioniq_6_2025 else IONIQ_6_FRICTION_JERK_DEADZONE
@@ -535,6 +539,13 @@ class LatControlTorque(LatControl):
             -low_speed_output_limit,
             low_speed_output_limit,
           ))
+      elif ioniq_5_active:
+        low_speed_output_limit = get_ioniq_5_low_speed_output_limit(setpoint, desired_lateral_jerk, CS.vEgo)
+        output_torque = float(np.clip(
+          output_torque,
+          -low_speed_output_limit,
+          low_speed_output_limit,
+        ))
       elif self.is_ram_1500 and output_torque * setpoint > 0.0:
         output_torque *= get_ram_1500_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
       elif self.is_kona_non_scc:
diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py
index 375f3611e..def8ca945 100644
--- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py
@@ -255,13 +255,20 @@ GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT = 0.14
 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH = 0.05
 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.0
 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5
-GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.14
+GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.06
 GENESIS_G70_CURVE_UNWIND_SPEED = 18.0
 GENESIS_G70_CURVE_UNWIND_SPEED_WIDTH = 3.0
 GENESIS_G70_CURVE_UNWIND_LAT = 0.25
 GENESIS_G70_CURVE_UNWIND_LAT_WIDTH = 0.12
 GENESIS_G70_CURVE_UNWIND_JERK = 0.08
 GENESIS_G70_CURVE_UNWIND_JERK_WIDTH = 0.08
+GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.20
+GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.12
+GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.12
+GENESIS_G70_UNWIND_FF_JERK = 0.10
+GENESIS_G70_UNWIND_FF_JERK_WIDTH = 0.10
+GENESIS_G70_UNWIND_FF_SPEED = 18.0
+GENESIS_G70_UNWIND_FF_SPEED_WIDTH = 3.0
 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.15
 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED = 50.0 * CV.MPH_TO_MS
 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS
@@ -669,6 +676,14 @@ IONIQ_5_SUSTAINED_TURN_IN_FF_SPEED_WIDTH = 1.8
 IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_START = 1.10
 IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_END = 3.60
 IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_WIDTH = 0.30
+IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE = 0.05
+IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF = 0.95
+IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED = 8.0
+IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 0.8
+IONIQ_5_LOW_SPEED_CENTER_LAT = 0.40
+IONIQ_5_LOW_SPEED_CENTER_LAT_WIDTH = 0.10
+IONIQ_5_LOW_SPEED_CENTER_JERK = 0.40
+IONIQ_5_LOW_SPEED_CENTER_JERK_WIDTH = 0.12
 
 IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.16
 IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.16
@@ -1055,6 +1070,16 @@ SUBARU_IMPREZA_PID_TAPER_START_DEG = 0.75
 SUBARU_IMPREZA_PID_TAPER_FULL_DEG = 4.0
 SUBARU_IMPREZA_PID_TAPER_MIN = 0.58
 
+RAV4_TSS2_CARS = (
+  TOYOTA_CAR.TOYOTA_RAV4_TSS2,
+)
+RAV4_TSS2_PID_LOW_SPEED = 12.0 * CV.MPH_TO_MS
+RAV4_TSS2_PID_LOW_SPEED_WIDTH = 2.0 * CV.MPH_TO_MS
+RAV4_TSS2_PID_CENTER_ANGLE = 14.0
+RAV4_TSS2_PID_CENTER_ANGLE_WIDTH = 3.0
+RAV4_TSS2_PID_OUTPUT_SCALE_MIN = 0.62
+RAV4_TSS2_PID_OUTPUT_ALPHA_MIN = 0.28
+
 RAM_1500_TRANSITION_TAPER_MAX = 0.34
 RAM_1500_TRANSITION_SPEED_ONSET = 10.0
 RAM_1500_TRANSITION_SPEED_FULL = 15.0
@@ -1539,6 +1564,21 @@ def get_subaru_impreza_pid_output_scale(angle_error_deg: float) -> float:
   return 1.0 - ((1.0 - SUBARU_IMPREZA_PID_TAPER_MIN) * error_weight)
 
 
+def get_rav4_tss2_pid_output(output_torque: float, prev_output_torque: float,
+                             desired_angle_deg: float, v_ego: float) -> float:
+  """Damp low-speed RAV4 center reversals without blunting real turns."""
+  speed_weight = _sigmoid((RAV4_TSS2_PID_LOW_SPEED - max(v_ego, 0.0)) /
+                          RAV4_TSS2_PID_LOW_SPEED_WIDTH)
+  center_weight = _sigmoid((RAV4_TSS2_PID_CENTER_ANGLE - abs(desired_angle_deg)) /
+                           RAV4_TSS2_PID_CENTER_ANGLE_WIDTH)
+  envelope = speed_weight * center_weight
+
+  output_scale = 1.0 - ((1.0 - RAV4_TSS2_PID_OUTPUT_SCALE_MIN) * envelope)
+  output_alpha = 1.0 - ((1.0 - RAV4_TSS2_PID_OUTPUT_ALPHA_MIN) * envelope)
+  limited_output = output_torque * output_scale
+  return float(prev_output_torque + output_alpha * (limited_output - prev_output_torque))
+
+
 def get_ram_1500_transition_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
   speed_weight = float(np.interp(v_ego, [RAM_1500_TRANSITION_SPEED_ONSET, RAM_1500_TRANSITION_SPEED_FULL], [0.0, 1.0]))
   jerk_weight = float(np.interp(abs(desired_lateral_jerk),
@@ -2752,6 +2792,23 @@ def get_genesis_g70_curve_unwind_output_scale(desired_lateral_accel: float, desi
   return 1.0 + GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST * speed_weight * lateral_weight * jerk_weight
 
 
+def get_genesis_g70_unwind_ff_scale(setpoint: float, measured_lateral_accel: float,
+                                    desired_lateral_jerk: float, v_ego: float) -> float:
+  if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0:
+    return 1.0
+
+  overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
+  if overshoot <= 0.0:
+    return 1.0
+  overshoot_weight = _sigmoid((overshoot - GENESIS_G70_UNWIND_FF_OVERSHOOT) /
+                              GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH)
+  jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_UNWIND_FF_JERK) /
+                         GENESIS_G70_UNWIND_FF_JERK_WIDTH)
+  speed_weight = _sigmoid((v_ego - GENESIS_G70_UNWIND_FF_SPEED) /
+                          GENESIS_G70_UNWIND_FF_SPEED_WIDTH)
+  return 1.0 - GENESIS_G70_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight * speed_weight
+
+
 def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_accel: float,
                                             desired_lateral_jerk: float, v_ego: float) -> float:
   tracking_error = abs(measured_lateral_accel - setpoint)
@@ -2861,6 +2918,22 @@ def get_ioniq_5_center_taper_scale(desired_lateral_accel: float, v_ego: float) -
   return 1.0 - reduction
 
 
+def get_ioniq_5_low_speed_output_limit(desired_lateral_accel: float,
+                                       desired_lateral_jerk: float, v_ego: float) -> float:
+  """Bound stop-transition center chatter without blunting actual turns."""
+  speed_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED - max(v_ego, 0.0)) /
+                                  IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH)
+  center_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_CENTER_LAT - abs(desired_lateral_accel)) /
+                                   IONIQ_5_LOW_SPEED_CENTER_LAT_WIDTH)
+  calm_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_CENTER_JERK - abs(desired_lateral_jerk)) /
+                                 IONIQ_5_LOW_SPEED_CENTER_JERK_WIDTH)
+  center_weight *= calm_weight
+  center_limit = (IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE +
+                  IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF * (1.0 - center_weight))
+  limit = 1.0 - speed_weight * (1.0 - center_limit)
+  return float(np.clip(limit, IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE, 1.0))
+
+
 def _ioniq_ev_old_sigmoid(x: float) -> float:
   return _sigmoid(x)
 
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 4ad9ca0e8..b6aea1117 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -295,6 +295,9 @@ class LongControl:
       a_target = self.vehicle_tuning.shape_toyota_sienna_accel_target(
         a_target, CS.vEgo, should_stop, leads=leads,
       )
+      a_target = self.vehicle_tuning.shape_hyundai_elantra_lead_target(
+        a_target, CS.vEgo, should_stop, leads,
+      )
       error = a_target - CS.aEgo
       self.update_mpc_mode(self.experimental_mode)
       self.vehicle_tuning.shape_volt_test_tune_integrator(self.pid, error, CS.vEgo)
@@ -327,6 +330,9 @@ class LongControl:
         should_stop,
         has_lead,
       )
+      raw_output_accel = self.vehicle_tuning.cap_hyundai_elantra_lead_output(
+        raw_output_accel, CS.vEgo, should_stop, leads,
+      )
 
       if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended':
         if raw_output_accel < 0 and raw_output_accel < self.last_output_accel:
diff --git a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py
index 1f5f1938e..64423247b 100644
--- a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py
+++ b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py
@@ -48,6 +48,10 @@ VOLT_CRUISE_INTEGRATOR_LEAK = 0.995
 SUBARU_IMPREZA_STOP_RELEASE_TIME = 0.75
 SUBARU_IMPREZA_STOP_RELEASE_MAX_ACCEL = 0.8
 HYUNDAI_ELANTRA_STOPPING_HOLD_TARGET_GAP = 0.25
+HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_EGO_SPEED = 2.0
+HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_SPEED = 0.5
+HYUNDAI_ELANTRA_STOPPED_LEAD_MIN_CLOSING_SPEED = 0.25
+HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_CREEP_ACCEL = 0.05
 
 
 def get_bolt_acc_pedal_friction_bias(output_accel, a_target, v_ego):
@@ -157,6 +161,31 @@ class LongControlVehicleTuning:
       return output_accel
     return max(float(output_accel), float(stop_accel))
 
+  def is_hyundai_elantra_closing_on_stopped_lead(self, v_ego, should_stop, leads):
+    if not self.is_hyundai_elantra_2021 or should_stop or v_ego > HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_EGO_SPEED:
+      return False
+
+    lead = leads[0] if leads else None
+    if lead is None or not bool(getattr(lead, "status", False)):
+      return False
+
+    lead_speed = max(0.0, float(getattr(lead, "vLead", 0.0)))
+    closing_speed = float(v_ego) - lead_speed
+    return (
+      lead_speed <= HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_SPEED and
+      closing_speed >= HYUNDAI_ELANTRA_STOPPED_LEAD_MIN_CLOSING_SPEED
+    )
+
+  def shape_hyundai_elantra_lead_target(self, a_target, v_ego, should_stop, leads):
+    if self.is_hyundai_elantra_closing_on_stopped_lead(v_ego, should_stop, leads):
+      return min(float(a_target), HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_CREEP_ACCEL)
+    return a_target
+
+  def cap_hyundai_elantra_lead_output(self, output_accel, v_ego, should_stop, leads):
+    if self.is_hyundai_elantra_closing_on_stopped_lead(v_ego, should_stop, leads):
+      return min(float(output_accel), HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_CREEP_ACCEL)
+    return output_accel
+
   def cap_subaru_stop_release_accel(self, output_accel, stopping_handoff, should_stop):
     """Prevent an Impreza stop-sign handoff from stepping straight into full throttle."""
     if not self.is_subaru_impreza_2020:
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 97df7f10e..08c2958b0 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -22,7 +22,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   get_far_follow_output_slew_rates,
   get_follow_prebrake_min_headway,
   is_gm_silverado_early_follow_lead,
-  is_toyota_rav4_tss2_2023,
+  is_toyota_rav4_tss2_post_departure_tune,
   get_toyota_sienna_post_departure_restop_cap,
   get_untracked_slow_lead_decel_scale,
 )
@@ -2663,7 +2663,7 @@ class LongitudinalPlanner:
     # cruise branch request full acceleration before the next lead was stable.
     # Keep the normal catch-up cap on this car; urgent braking remains outside
     # this comfort policy and is still allowed through unchanged.
-    post_departure_bypass = post_departure_active and not is_toyota_rav4_tss2_2023(self.CP)
+    post_departure_bypass = post_departure_active and not is_toyota_rav4_tss2_post_departure_tune(self.CP)
     follow_result = apply_follow_policy(
       self.lead_one,
       self.lead_two,
diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py
index b428adae6..c83db3bc4 100644
--- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py
+++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py
@@ -20,11 +20,11 @@ TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_BRAKE = 0.18
 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_BRAKE = 0.32
 
 
-def is_toyota_rav4_tss2_2023(CP):
-  """Identify the RAV4 TSS2 controller that needs normal catch-up caps after departure."""
+def is_toyota_rav4_tss2_post_departure_tune(CP):
+  """Identify RAV4 TSS2 variants that need normal catch-up caps after departure."""
   return (
     getattr(CP, "brand", "") == "toyota" and
-    str(getattr(CP, "carFingerprint", "")) == "TOYOTA_RAV4_TSS2_2023"
+    str(getattr(CP, "carFingerprint", "")) in ("TOYOTA_RAV4_TSS2", "TOYOTA_RAV4_TSS2_2023")
   )
 
 
diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py
index 288317a31..d327a3af0 100644
--- a/selfdrive/controls/tests/test_latcontrol.py
+++ b/selfdrive/controls/tests/test_latcontrol.py
@@ -41,6 +41,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
   get_gmc_yukon_cc_ff_scale,
   get_ram_1500_transition_output_scale,
   get_ram_1500_ff_scale,
+  get_rav4_tss2_pid_output,
   get_subaru_impreza_pid_output_scale,
   normalize_flm_overrides,
   set_flm_runtime_overrides,
@@ -78,6 +79,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   get_genesis_g70_high_speed_error_scale,
   get_genesis_g70_low_speed_angle_damping,
   get_genesis_g70_low_speed_output_limit,
+  get_genesis_g70_unwind_ff_scale,
   get_genesis_gv70_friction_threshold,
   get_genesis_gv70_high_speed_error_scale,
   get_genesis_gv70_unwind_ff_scale,
@@ -107,6 +109,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   get_ioniq_5_friction_scale,
   get_ioniq_5_friction_threshold,
   get_ioniq_5_center_taper_scale,
+  get_ioniq_5_low_speed_output_limit,
   get_ioniq_ev_old_center_taper_scale,
   get_ioniq_ev_old_ff_scale,
   get_ioniq_6_center_taper_scale,
@@ -822,6 +825,9 @@ class TestLatControl:
     assert get_genesis_g70_low_speed_angle_damping(0.0, 20.0, 0.0, 2.0) > 0.0
     assert get_genesis_g70_curve_unwind_output_scale(0.7, -0.5, 25.0) > 1.0
     assert get_genesis_g70_curve_unwind_output_scale(0.7, 0.5, 25.0) == 1.0
+    assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, 0.5, 25.0) < 1.0
+    assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, -0.5, 25.0) == 1.0
+    assert get_genesis_g70_unwind_ff_scale(-0.7, 0.2, 0.5, 25.0) == 1.0
 
     assert get_genesis_g70_high_speed_error_scale(0.2, 0.2, 0.8, 20.0) == 1.0
     assert get_genesis_g70_high_speed_error_scale(0.2, 0.9, 0.8, 20.0) < 1.0
@@ -1112,6 +1118,15 @@ class TestLatControl:
     assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.0, 10.0)
     assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.20, 25.0) <= 1.0
 
+  def test_ioniq_5_low_speed_output_limit_preserves_turn_relief(self):
+    center = get_ioniq_5_low_speed_output_limit(0.02, 0.05, 3.0)
+    turn = get_ioniq_5_low_speed_output_limit(0.70, 0.70, 3.0)
+    highway = get_ioniq_5_low_speed_output_limit(0.02, 0.05, 15.0)
+
+    assert center < 0.20
+    assert turn > center
+    assert highway > center
+
   def test_ioniq_ev_old_ff_scale_curve(self):
     assert get_ioniq_ev_old_ff_scale(0.0, 0.0, 20.0) == 1.0
     assert get_ioniq_ev_old_ff_scale(0.35, 0.0, 20.0) > get_ioniq_ev_old_ff_scale(-0.35, 0.0, 20.0)
@@ -1444,6 +1459,18 @@ class TestLatControl:
     assert lac_log.active
     assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor * 1.22)
 
+  def test_ioniq_5_low_speed_output_guard_update_path(self, monkeypatch):
+    monkeypatch.setattr(latcontrol_torque, "get_ioniq_5_low_speed_output_limit", lambda *_args: 0.05)
+    controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_5)
+    CS.vEgo = 3.2
+
+    output, _, lac_log = controller.update(
+      True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles,
+    )
+
+    assert lac_log.active
+    assert abs(output) <= 0.05
+
   def test_ioniq_6_default_update_path(self):
     controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_6)
 
@@ -1612,6 +1639,31 @@ class TestLatControl:
     assert get_subaru_impreza_pid_output_scale(4.0) == pytest.approx(0.58)
     assert get_subaru_impreza_pid_output_scale(-4.0) == pytest.approx(0.58)
 
+  def test_rav4_tss2_pid_output_damps_low_speed_center_reversals(self):
+    low_speed = get_rav4_tss2_pid_output(1.0, -1.0, 4.0, 6.0 * 0.44704)
+    large_turn = get_rav4_tss2_pid_output(1.0, -1.0, 24.0, 6.0 * 0.44704)
+    highway = get_rav4_tss2_pid_output(1.0, -1.0, 4.0, 25.0 * 0.44704)
+
+    assert abs(low_speed) < 0.50
+    assert abs(large_turn) > abs(low_speed)
+    assert highway > low_speed
+
+  def test_rav4_tss2_pid_output_update_path(self, monkeypatch):
+    controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(TOYOTA.TOYOTA_RAV4_TSS2)
+    CS.vEgo = 6.0 * 0.44704
+    CS.steeringAngleDeg = 8.0
+
+    tuned_output, _, lac_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, starpilot_toggles)
+
+    monkeypatch.setattr(latcontrol_pid, "get_rav4_tss2_pid_output", lambda output, *_args: output)
+    base_controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(TOYOTA.TOYOTA_RAV4_TSS2)
+    CS.vEgo = 6.0 * 0.44704
+    CS.steeringAngleDeg = 8.0
+    base_output, _, _ = base_controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, starpilot_toggles)
+
+    assert lac_log.active
+    assert abs(tuned_output) < abs(base_output)
+
   def test_subaru_impreza_pid_output_taper_path(self, monkeypatch):
     controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(SUBARU.SUBARU_IMPREZA)
     CS.steeringAngleDeg = 3.0
diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py
index b2b5d0834..dc26fe0dd 100644
--- a/selfdrive/controls/tests/test_longcontrol.py
+++ b/selfdrive/controls/tests/test_longcontrol.py
@@ -764,6 +764,31 @@ def test_elantra_lead_stop_releases_stale_hard_brake_after_target_eases():
   assert tuning.shape_stopping_accel(-1.20, -0.25, True, 1.0, False, -0.85) == pytest.approx(-1.20)
 
 
+def test_elantra_stopped_lead_handoff_holds_braking_direction_without_touching_brakes():
+  CP = make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021")
+  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
+  stopped_lead = SimpleNamespace(status=True, vLead=0.1, dRel=14.0)
+
+  assert tuning.shape_hyundai_elantra_lead_target(0.14, 1.1, False, (stopped_lead,)) == pytest.approx(0.05)
+  assert tuning.cap_hyundai_elantra_lead_output(0.14, 1.1, False, (stopped_lead,)) == pytest.approx(0.05)
+  assert tuning.cap_hyundai_elantra_lead_output(-0.5, 1.1, False, (stopped_lead,)) == pytest.approx(-0.5)
+
+
+def test_elantra_stopped_lead_handoff_releases_for_moving_lead_and_other_cars():
+  elantra = vehicle_tunes.LongControlVehicleTuning(
+    make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021")
+  )
+  other_car = vehicle_tunes.LongControlVehicleTuning(
+    make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_SONATA")
+  )
+  moving_lead = SimpleNamespace(status=True, vLead=0.8, dRel=14.0)
+  stopped_lead = SimpleNamespace(status=True, vLead=0.1, dRel=14.0)
+
+  assert elantra.shape_hyundai_elantra_lead_target(0.14, 1.1, False, (moving_lead,)) == pytest.approx(0.14)
+  assert elantra.shape_hyundai_elantra_lead_target(0.14, 1.1, True, (stopped_lead,)) == pytest.approx(0.14)
+  assert other_car.shape_hyundai_elantra_lead_target(0.14, 1.1, False, (stopped_lead,)) == pytest.approx(0.14)
+
+
 def test_volt_testing_ground_handoff_freezes_integrator(monkeypatch):
   CP = car.CarParams.new_message()
   CP.brand = "gm"
diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py
index bd2e5efd5..699cc2206 100644
--- a/selfdrive/controls/tests/test_longitudinal_planner.py
+++ b/selfdrive/controls/tests/test_longitudinal_planner.py
@@ -22,7 +22,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   get_follow_prebrake_min_headway,
   get_toyota_sienna_post_departure_restop_cap,
   is_gm_silverado_early_follow_lead,
-  is_toyota_rav4_tss2_2023,
+  is_toyota_rav4_tss2_post_departure_tune,
 )
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 from openpilot.selfdrive.modeld import modeld
@@ -2770,12 +2770,14 @@ def test_publish_has_lead_includes_second_mpc_lead():
   assert pm.sent["longitudinalPlan"].longitudinalPlan.hasLead
 
 
-def test_rav4_tss2_2023_is_the_car_specific_post_departure_tune():
-  rav4_cp = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
+def test_rav4_tss2_variants_use_the_car_specific_post_departure_tune():
+  rav4_2019_cp = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2)
+  rav4_2023_cp = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
   other_cp = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2022)
 
-  assert is_toyota_rav4_tss2_2023(rav4_cp)
-  assert not is_toyota_rav4_tss2_2023(other_cp)
+  assert is_toyota_rav4_tss2_post_departure_tune(rav4_2019_cp)
+  assert is_toyota_rav4_tss2_post_departure_tune(rav4_2023_cp)
+  assert not is_toyota_rav4_tss2_post_departure_tune(other_cp)
 
 
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py
index 1bd5b851b..a0559f9d1 100755
--- a/selfdrive/modeld/modeld.py
+++ b/selfdrive/modeld/modeld.py
@@ -122,6 +122,12 @@ def _canonical_model_id(model_id: str) -> str:
   return MODEL_ID_ALIASES.get(key, key)
 
 
+def _select_builtin_model(params: Params) -> None:
+  params.put("Model", BUILTIN_MODEL_KEY)
+  params.put("DrivingModel", BUILTIN_MODEL_KEY)
+  params.put("DrivingModelName", "Regret Driven Framework")
+
+
 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
                           lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool,
                           is_v9: bool, is_v14: bool, is_v15: bool, starpilot_toggles,
@@ -448,6 +454,24 @@ class ModelState:
     return parsed
 
 
+def _load_model_state(cam_w: int, cam_h: int, selected_model: str, external_gpu_requested: bool,
+                      params: Params) -> ModelState:
+  try:
+    return ModelState(cam_w, cam_h, external_gpu_requested)
+  except Exception:
+    if selected_model == BUILTIN_MODEL_KEY:
+      raise
+
+    cloudlog.exception(f"Failed to load model {selected_model}; falling back to {BUILTIN_MODEL_KEY}")
+    _select_builtin_model(params)
+    if external_gpu_requested:
+      from tinygrad.helpers import DEV
+      device_config = tinygrad_dev_config(False, TICI)
+      DEV.value = device_config
+      os.environ["DEV"] = device_config
+    return ModelState(cam_w, cam_h, False)
+
+
 def main(demo=False):
   cloudlog.warning("modeld init")
 
@@ -500,16 +524,7 @@ def main(demo=False):
   cloudlog.warning("loading model")
   if external_gpu_requested:
     wait_usbgpu_link()
-  try:
-    model = ModelState(vipc_client_main.width, vipc_client_main.height, external_gpu_requested)
-  except Exception:
-    if not external_gpu_requested:
-      raise
-    cloudlog.exception(f"Failed to load external-GPU model {selected_model}; falling back to {BUILTIN_MODEL_KEY}")
-    device_config = tinygrad_dev_config(False, TICI)
-    DEV.value = device_config
-    os.environ["DEV"] = device_config
-    model = ModelState(vipc_client_main.width, vipc_client_main.height, False)
+  model = _load_model_state(vipc_client_main.width, vipc_client_main.height, selected_model, external_gpu_requested, params)
   external_gpu_active = model.uses_external_gpu
   params.put_bool("UsbGpuCompiled", external_model_selected and file_chunked_exists(external_artifact))
   params.put_bool("UsbGpuActive", external_gpu_active)
diff --git a/selfdrive/modeld/tests/test_model_fallback.py b/selfdrive/modeld/tests/test_model_fallback.py
new file mode 100644
index 000000000..609dba843
--- /dev/null
+++ b/selfdrive/modeld/tests/test_model_fallback.py
@@ -0,0 +1,41 @@
+import pytest
+
+from openpilot.selfdrive.modeld import modeld
+
+
+class FakeParams:
+  def __init__(self):
+    self.values = {}
+
+  def put(self, key, value):
+    self.values[key] = value
+
+
+def test_incompatible_downloaded_model_falls_back_to_builtin(monkeypatch):
+  calls = []
+  builtin_model = object()
+
+  def load_model(cam_w, cam_h, external_gpu_active):
+    calls.append((cam_w, cam_h, external_gpu_active))
+    if len(calls) == 1:
+      raise TypeError("incompatible artifact")
+    return builtin_model
+
+  params = FakeParams()
+  monkeypatch.setattr(modeld, "ModelState", load_model)
+  monkeypatch.setattr(modeld.cloudlog, "exception", lambda *_args, **_kwargs: None)
+
+  assert modeld._load_model_state(1928, 1208, "custom-model", False, params) is builtin_model
+  assert calls == [(1928, 1208, False), (1928, 1208, False)]
+  assert params.values == {
+    "Model": modeld.BUILTIN_MODEL_KEY,
+    "DrivingModel": modeld.BUILTIN_MODEL_KEY,
+    "DrivingModelName": "Regret Driven Framework",
+  }
+
+
+def test_builtin_model_load_failure_is_not_hidden(monkeypatch):
+  monkeypatch.setattr(modeld, "ModelState", lambda *_args: (_ for _ in ()).throw(TypeError("bad builtin")))
+
+  with pytest.raises(TypeError, match="bad builtin"):
+    modeld._load_model_state(1928, 1208, modeld.BUILTIN_MODEL_KEY, False, FakeParams())