waffles

opendbc_repo/MANIFEST.in

@@ -1,3 +1,4 @@
 include opendbc/car/car.capnp
 include opendbc/car/include/c++.capnp
+include opendbc/dbc/hyundai_kia_ray_pedal.dbc
 recursive-include opendbc/safety *.h

opendbc_repo/opendbc/car/__init__.py

@@ -90,6 +90,7 @@ class Bus(StrEnum):
   main = auto()
   party = auto()
   ap_party = auto()
+  ap_pt = auto()
 
 
 def rate_limit(new_value, last_value, dw_step, up_step):

opendbc_repo/opendbc/car/hyundai/carcontroller.py

@@ -4,7 +4,7 @@ from dataclasses import dataclass
 # hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md and THIRD_PARTY_NOTICES.md.
 import numpy as np
 from opendbc.can import CANPacker
-from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, rate_limit, structs
+from opendbc.car import Bus, DT_CTRL, create_gas_interceptor_command, make_tester_present_msg, rate_limit, structs
 from opendbc.car.common.filter_simple import FirstOrderFilter
 from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance, get_max_angle_delta_vm, get_max_angle_vm
 from opendbc.car.common.conversions import Conversions as CV
@@ -42,6 +42,9 @@ IONIQ_6_RESPONSE_MULTIPLIER = 1.2
 IONIQ_6_CANFD_SCC_ACCEL_STEP = (6.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER
 IONIQ_6_CANFD_SCC_DECEL_STEP = (15.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER
 EV9_CANFD_SCC_DECEL_STEP = 10.0 / 50.0
+RAY_PEDAL_COMMAND_CAP = 0.35  # Ray firmware voltage scaling is route-derived; validate before raising.
+RAY_PEDAL_RATE_UP = 0.012     # per 25 Hz command (0.30 normalized pedal per second)
+RAY_PEDAL_RATE_DOWN = 0.06
 GENESIS_G90_STOP_HOLD_SPEED_BP = [0.0, 0.03, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0]
 GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.10, -0.10, -0.12, -0.18, -0.30, -0.50, -0.75, -1.00, -1.40, -1.80]
 GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP = [0.0, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0]
@@ -485,6 +488,9 @@ class CarController(CarControllerBase):
       CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_REFRESH_MSGS
     )
     self._ray_lfa_packer = CANPacker("hyundai_kia_ray_lfa") if self._ray_lfa_8byte else None
+    self._ray_pedal = CP.carFingerprint == CAR.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED
+    self._ray_pedal_packer = CANPacker("hyundai_kia_ray_pedal") if self._ray_pedal else None
+    self._ray_pedal_gas_last = 0.0
 
   def _update_dash_icon_state(self, CC):
     if CC.latActive:
@@ -811,7 +817,11 @@ class CarController(CarControllerBase):
     if not self.long_active_ecu:
       if self.cancel_counter > CANCEL_BUTTON_DELAY_FRAMES:
         can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
-      elif CC.cruiseControl.resume:
+      elif self._ray_pedal and CC.longActive and CS.out.cruiseState.enabled:
+        if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
+          can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
+          self.last_button_frame = self.frame
+      elif CC.cruiseControl.resume and not self._ray_pedal:
         # send resume at a max freq of 10Hz
         if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
           # send 25 messages at a time to increases the likelihood of resume being accepted
@@ -819,7 +829,24 @@ class CarController(CarControllerBase):
           if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
             self.last_button_frame = self.frame
       else:
-        can_sends.extend(self._create_can_redneck_button_messages(CS))
+        if not self._ray_pedal:
+          can_sends.extend(self._create_can_redneck_button_messages(CS))
+
+    if self._ray_pedal and self.frame % 4 == 0:
+      pedal_ready = CS.ray_pedal_valid and CS.ray_pedal_state == 0
+      pedal_active = (CC.longActive and pedal_ready and not CC.cruiseControl.override and
+                      not CS.out.gasPressed and not CS.out.brakePressed and
+                      not CS.out.cruiseState.enabled and CS.out.vEgo >= self.CP.minEnableSpeed)
+      if pedal_active:
+        target = float(np.clip(accel / CarControllerParams.ACCEL_MAX * RAY_PEDAL_COMMAND_CAP,
+                               0.0, RAY_PEDAL_COMMAND_CAP))
+        self._ray_pedal_gas_last = rate_limit(
+          target, self._ray_pedal_gas_last, -RAY_PEDAL_RATE_DOWN, RAY_PEDAL_RATE_UP,
+        )
+      else:
+        self._ray_pedal_gas_last = 0.0
+      can_sends.append(create_gas_interceptor_command(
+        self._ray_pedal_packer, self._ray_pedal_gas_last, (self.frame // 4) & 0xF))
 
     if self.long_active_ecu and can_canfd_blended:
       if blended_hda2:

opendbc_repo/opendbc/car/hyundai/carstate.py

@@ -138,6 +138,9 @@ class CarState(CarStateBase):
     self.buttons_counter = 0
     self.main_cruise_on = False
     self.main_cruise_tracking = bool(getattr(FPCP, "flags", 0) & HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING)
+    if CP.carFingerprint == CAR.KIA_RAY_EV:
+      self.ray_pedal_state = 5
+      self.ray_pedal_valid = False
 
     self.cruise_info = {}
     self.msg_161 = {}
@@ -300,6 +303,7 @@ class CarState(CarStateBase):
     cp = can_parsers[Bus.pt]
     cp_cam = can_parsers[Bus.cam]
     cp_alt = can_parsers.get(Bus.alt)
+    cp_pedal = can_parsers.get(Bus.party)
 
     if self.CP.flags & HyundaiFlags.CANFD:
       return self.update_canfd(can_parsers)
@@ -393,6 +397,11 @@ class CarState(CarStateBase):
     ret.espDisabled = cp.vl["TCS11"]["TCS_PAS"] == 1
     ret.espActive = cp.vl["TCS11"]["ABS_ACT"] == 1
     ret.accFaulted = False if no_scc else cp.vl["TCS13"]["ACCEnable"] != 0  # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
+    if self.CP.carFingerprint == CAR.KIA_RAY_EV and self.CP.enableGasInterceptorDEPRECATED:
+      self.ray_pedal_valid = bool(cp_pedal is not None and cp_pedal.can_valid and
+                                  cp_pedal.ts_nanos["GAS_SENSOR"]["STATE"] > 0)
+      self.ray_pedal_state = int(cp_pedal.vl["GAS_SENSOR"]["STATE"]) if cp_pedal is not None else 5
+      ret.accFaulted = not self.ray_pedal_valid or self.ray_pedal_state != 0
 
     if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV | HyundaiFlags.FCEV):
       if self.CP.flags & HyundaiFlags.FCEV:
@@ -748,4 +757,6 @@ class CarState(CarStateBase):
     }
     if CP.carFingerprint in ALT_BUS_LDA_BUTTON_CARS:
       parsers[Bus.alt] = CANParser(DBC[CP.carFingerprint][Bus.pt], [("CLU13", 0)], 1)
+    if CP.carFingerprint == CAR.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED:
+      parsers[Bus.party] = CANParser("hyundai_kia_ray_pedal", [("GAS_SENSOR", 50)], 0)
     return parsers

opendbc_repo/opendbc/car/hyundai/interface.py

@@ -43,6 +43,7 @@ ENABLE_BUTTONS = (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.can
 ECU_DISABLE_TIMESTAMP = 0.0
 KONA_NON_SCC_FCA_RADAR_ADDR = 0x602
 KIA_EV9_ACCEL_MAX = 2.2
+RAY_PEDAL_SENSOR_ADDR = 0x201
 
 
 def apply_platform_longitudinal_params(ret: structs.CarParams) -> None:
@@ -302,6 +303,18 @@ class CarInterface(CarInterfaceBase):
     elif ret.flags & HyundaiFlags.FCEV:
       ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.FCEV_GAS.value
 
+    if (candidate == CAR.KIA_RAY_EV and fingerprint[0].get(RAY_PEDAL_SENSOR_ADDR) == 6 and
+        ret.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_REFRESH_MSGS and
+        ret.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON):
+      ret.enableGasInterceptorDEPRECATED = True
+      ret.alphaLongitudinalAvailable = True
+      ret.openpilotLongitudinalControl = True
+      ret.pcmCruise = False
+      ret.radarUnavailable = True
+      ret.autoResumeSng = False
+      ret.minEnableSpeed = 5.0  # pedal-only: no commanded friction brake/standstill hold
+      ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.LONG.value
+
     # Car specific configuration overrides
 
     if candidate == CAR.GENESIS_G90:

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

@@ -0,0 +1,133 @@
+from types import SimpleNamespace
+
+import pytest
+
+from opendbc.can import CANPacker, CANParser
+from opendbc.car import Bus, gen_empty_fingerprint
+from opendbc.car.hyundai.carcontroller import CarController
+from opendbc.car.hyundai.carstate import CarState
+from opendbc.car.hyundai.interface import CarInterface
+from opendbc.car.hyundai.values import CAR, DBC, HyundaiSafetyFlags
+from opendbc.car.structs import CarControl
+
+
+def ray_fingerprint(sensor_length=6, lfa_length=8):
+  fingerprint = gen_empty_fingerprint()
+  fingerprint[0][0x201] = sensor_length
+  fingerprint[0][0x391] = 8
+  fingerprint[2][0x485] = lfa_length
+  return fingerprint
+
+
+@pytest.mark.parametrize(("candidate", "fingerprint", "has_pedal"), [
+  (CAR.KIA_RAY_EV, ray_fingerprint(), True),
+  (CAR.KIA_RAY_EV, ray_fingerprint(sensor_length=8), False),
+  (CAR.KIA_RAY_EV, ray_fingerprint(lfa_length=4), False),
+  (CAR.HYUNDAI_KONA_EV_NON_SCC, ray_fingerprint(), False),
+])
+def test_ray_pedal_fingerprint_isolation(candidate, fingerprint, has_pedal):
+  CP = CarInterface.get_params(candidate, fingerprint, [], False, False, False, None)
+  assert CP.enableGasInterceptorDEPRECATED is has_pedal
+  assert CP.openpilotLongitudinalControl is has_pedal
+  if has_pedal:
+    assert not CP.pcmCruise
+    assert CP.safetyConfigs[-1].safetyParam == 0x9405
+    assert CP.minEnableSpeed == 5.0
+    assert not CP.autoResumeSng
+    FPCP = CarInterface.get_starpilot_params(candidate, fingerprint, [], CP, SimpleNamespace())
+    assert FPCP.canUsePedal
+    assert not FPCP.pcmCruiseSpeed
+    assert not FPCP.redneckCruiseAvailable
+  else:
+    assert CP.pcmCruise
+    assert not (CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG)
+
+
+def test_ray_pedal_safety_signature_is_unique_across_hyundai_platforms():
+  for candidate in CAR:
+    for alpha_long in (False, True):
+      CP = CarInterface.get_params(candidate, ray_fingerprint(), [],
+                                   alpha_long, False, False, None)
+      has_ray_signature = (CP.safetyConfigs[-1].safetyParam & ~(32 | 128 | 2048)) == 0x9405
+      assert has_ray_signature is (candidate == CAR.KIA_RAY_EV)
+      assert CP.enableGasInterceptorDEPRECATED is (candidate == CAR.KIA_RAY_EV)
+
+
+def test_ray_pedal_parser_validates_actual_route_frames():
+  CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None)
+  parser = CarState(CP, None).get_can_parsers(CP)[Bus.party]
+  assert parser.dbc_name == "hyundai_kia_ray_pedal"
+  # Consecutive bus-0 GAS_SENSOR frames from Sept. 15 Ray rlog segment 4.
+  samples = [bytes.fromhex(s) for s in (
+    "01f403d55de8", "01f603d55ef1", "01f403d55f51", "01f603d3503f",
+    "01f903d551ab", "01f903d552a4", "01f703d55370",
+  )]
+  for idx, dat in enumerate(samples):
+    parser.update([(1_000_000_000 + idx * 20_000_000, [(0x201, dat, 0)])])
+    assert parser.can_valid
+    assert parser.vl["GAS_SENSOR"]["STATE"] == 5  # FAULT_TIMEOUT: no 0x200 was sent
+
+  prior = parser.vl_raw["GAS_SENSOR"]
+  bad = bytearray(samples[-1])
+  bad[-1] ^= 1
+  parser.update([(1_160_000_000, [(0x201, bytes(bad), 0)])])
+  assert parser.vl_raw["GAS_SENSOR"] == prior
+
+
+def test_ray_pedal_fault_clears_only_with_healthy_sensor_state():
+  CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None)
+  state = CarState(CP, None)
+  parsers = state.get_can_parsers(CP)
+  packer = CANPacker("hyundai_kia_ray_pedal")
+  sensor = packer.make_can_msg("GAS_SENSOR", 0, {
+    "INTERCEPTOR_GAS": 0, "INTERCEPTOR_GAS2": 0,
+    "STATE": 0, "COUNTER_PEDAL": 1,
+  })
+  for parser in parsers.values():
+    parser.update([(1_000_000_000, [sensor])])
+  ret, _ = state.update(parsers, SimpleNamespace())
+  assert state.ray_pedal_valid
+  assert state.ray_pedal_state == 0
+  assert not ret.accFaulted
+
+
+def test_ray_controller_heartbeats_and_only_actuates_when_ready():
+  CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None)
+  controller = CarController(DBC[CP.carFingerprint], CP)
+  parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0), ("CLU11", 0)], 0)
+  CS = SimpleNamespace(
+    lkas11=parser.vl["LKAS11"], clu11=parser.vl["CLU11"],
+    out=SimpleNamespace(vEgo=12.0, gasPressed=False, brakePressed=False,
+                        cruiseState=SimpleNamespace(enabled=False)),
+    ray_pedal_valid=True, ray_pedal_state=5, is_metric=True,
+  )
+  CC = SimpleNamespace(
+    enabled=True, longActive=True, latActive=True,
+    cruiseControl=SimpleNamespace(cancel=False, resume=False, override=False),
+  )
+  hud = SimpleNamespace(
+    visualAlert=CarControl.HUDControl.VisualAlert.none,
+    leftLaneVisible=True, rightLaneVisible=True,
+    leftLaneDepart=False, rightLaneDepart=False,
+  )
+  actuators = SimpleNamespace(longControlState=CarControl.Actuators.LongControlState.pid)
+
+  def pedal_msg(accel, frame):
+    controller.frame = frame
+    messages = controller.create_can_msgs(True, 0, False, 0.0, accel, False,
+                                          hud, actuators, CS, CC, 2, 0)
+    return next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0)
+
+  assert pedal_msg(2.0, 0)[:4] == bytes(4)  # fault timeout: heartbeat only
+  CS.ray_pedal_state = 0
+  assert pedal_msg(2.0, 4)[:4] != bytes(4)
+  CS.out.gasPressed = True
+  assert pedal_msg(2.0, 8)[:4] == bytes(4)
+  CS.out.gasPressed = False
+  assert pedal_msg(-1.0, 12)[:4] == bytes(4)  # decel = EV lift/regen, not gas
+  CS.out.cruiseState.enabled = True
+  controller.frame = 16
+  messages = controller.create_can_msgs(True, 0, False, 0.0, 2.0, False,
+                                        hud, actuators, CS, CC, 2, 0)
+  assert next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0)[:4] == bytes(4)
+  assert any(addr == 0x4F1 and bus == 0 for addr, _, bus in messages)  # cancel stock CC

opendbc_repo/opendbc/car/interfaces.py

@@ -233,6 +233,9 @@ class CarInterfaceBase(ABC):
           fp_ret.flags |= int(HondaStarPilotFlags.HAS_CAMERA_MESSAGES)
 
       elif platform in HYUNDAI:
+        if candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED:
+          fp_ret.canUsePedal = True
+          fp_ret.pcmCruiseSpeed = False
         if candidate in CANFD_CAR:
           hda2 = Ecu.adas in [fw.ecu for fw in car_fw]
           CAN = CanBus(None, fingerprint, bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING))
@@ -244,7 +247,9 @@ class CarInterfaceBase(ABC):
         if candidate != HYUNDAI.KIA_RAY_EV and not (CP.flags & HyundaiFlags.CANFD) and 0x53E in fingerprint[2]:
           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)
+        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))
         if fp_ret.redneckCruiseAvailable and params.get_bool("RedneckCruise"):
           fp_ret.pcmCruiseSpeed = False
           CP.openpilotLongitudinalControl = True

opendbc_repo/opendbc/car/tesla/carcontroller.py

@@ -5,9 +5,10 @@ from opendbc.car.lateral import apply_steer_angle_limits_vm
 from opendbc.car.interfaces import CarControllerBase
 from opendbc.car.tesla.coop_steering import CooperativeSteeringController
 from opendbc.car.tesla.teslacan import TeslaCAN
+from opendbc.car.tesla.teslacan_legacy import TeslaCANRaven
 from opendbc.car.tesla.preap.carcontroller import PreAPLongController, init_preap_can
 from opendbc.car.tesla.preap.stock_cc_spoofer import StockCCSpoofer
-from opendbc.car.tesla.values import CANBUS, CAR, CarControllerParams, TeslaSafetyFlags
+from opendbc.car.tesla.values import CANBUS, CAR, CarControllerParams, TeslaSafetyFlags, LEGACY_CARS
 from opendbc.car.vehicle_model import VehicleModel
 
 def get_safety_CP():
@@ -39,6 +40,13 @@ class CarController(CarControllerBase):
       self.stock_cc = StockCCSpoofer()
       from opendbc.car.tesla.interface import CarInterface
       self.VM = VehicleModel(CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP))
+    elif CP.carFingerprint in LEGACY_CARS:
+      self.packers = {
+        CANBUS.party: CANPacker(dbc_names[Bus.party]),
+      }
+      self.tesla_can = TeslaCANRaven(self.packers)
+      from opendbc.car.tesla.interface import CarInterface
+      self.VM = VehicleModel(CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1))
 
   def _clear_steering_limit_info(self):
     self.steering_limit_info_valid = False
@@ -104,9 +112,13 @@ class CarController(CarControllerBase):
         else:
           self._clear_steering_limit_info()
 
-      can_sends.append(self.tesla_can.create_steering_control(self.apply_angle_command_last, lat_active))
+      if self.CP.carFingerprint in LEGACY_CARS:
+        cntr = (self.frame // 2) % 16
+        can_sends.append(self.tesla_can.create_steering_control(cntr, self.apply_angle_command_last, lat_active))
+      else:
+        can_sends.append(self.tesla_can.create_steering_control(self.apply_angle_command_last, lat_active))
 
-    if self.frame % 10 == 0:
+    if self.frame % 10 == 0 and self.CP.carFingerprint not in LEGACY_CARS:
       can_sends.append(self.tesla_can.create_steering_allowed())
 
     # Longitudinal control
@@ -115,13 +127,21 @@ class CarController(CarControllerBase):
         state = 13 if CC.cruiseControl.cancel else 4  # 4=ACC_ON, 13=ACC_CANCEL_GENERIC_SILENT
         accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
         cntr = (self.frame // 4) % 8
-        can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, self.frame, CS.out.vEgo, CS.out.gasPressed))
+        if self.CP.carFingerprint in LEGACY_CARS:
+          hw1_accel = accel if CC.longActive and not CC.cruiseControl.cancel else 0.
+          hw1_active = CC.longActive and not CC.cruiseControl.cancel
+          can_sends.append(self.tesla_can.create_longitudinal_command(state, hw1_accel, cntr, CS.out.vEgo, hw1_active, CS.out.gasPressed))
+        else:
+          can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, self.frame, CS.out.vEgo, CS.out.gasPressed))
 
     else:
       # Increment counter so cancel is prioritized even without openpilot longitudinal
       if CC.cruiseControl.cancel:
         cntr = (CS.das_control["DAS_controlCounter"] + 1) % 8
-        can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, self.frame, CS.out.vEgo, False))
+        if self.CP.carFingerprint in LEGACY_CARS:
+          can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, CS.out.vEgo, False, CS.out.gasPressed))
+        else:
+          can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, self.frame, CS.out.vEgo, False))
 
     # TODO: HUD control
     new_actuators = actuators.as_builder()

opendbc_repo/opendbc/car/tesla/carstate.py

@@ -4,7 +4,10 @@ from opendbc.can import CANDefine, CANParser
 from opendbc.car import Bus, structs
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.interfaces import CarStateBase
-from opendbc.car.tesla.values import DBC, CANBUS, GEAR_MAP, STEER_DISENGAGE_THRESHOLD, STEER_THRESHOLD, TeslaSafetyFlags, CAR
+from opendbc.car.tesla.values import (
+  DBC, CANBUS, GEAR_MAP, STEER_DISENGAGE_THRESHOLD, STEER_THRESHOLD, TeslaSafetyFlags,
+  CAR, LEGACY_CARS,
+)
 from opendbc.car.tesla.preap.carstate import get_preap_can_parsers, update_preap
 from opendbc.car.tesla.preap.engagement import PreAPEngagement
 from opendbc.car.tesla.preap.nap_conf import nap_conf
@@ -25,8 +28,19 @@ class CarState(CarStateBase):
   def __init__(self, CP, FPCP):
     super().__init__(CP, FPCP)
     self.can_define = CANDefine(DBC[CP.carFingerprint][Bus.party])
-    self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"] if CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP else \
-                          self.can_define.dv["DI_torque2"]["DI_gear"]
+    if CP.carFingerprint in LEGACY_CARS:
+      self.can_define_party = CANDefine(DBC[CP.carFingerprint][Bus.party])
+      self.can_define_pt = CANDefine(DBC[CP.carFingerprint][Bus.pt])
+      self.can_define_chassis = CANDefine(DBC[CP.carFingerprint][Bus.chassis])
+      self.can_defines = {
+        **self.can_define_party.dv,
+        **self.can_define_pt.dv,
+        **self.can_define_chassis.dv,
+      }
+      self.shifter_values = self.can_defines["DI_torque2"]["DI_gear"]
+    else:
+      self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"] if CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP else \
+                            self.can_define.dv["DI_torque2"]["DI_gear"]
 
     self.autopark = False
     self.autopark_prev = False
@@ -75,6 +89,8 @@ class CarState(CarStateBase):
   def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
     if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
       return update_preap(self, can_parsers)
+    if self.CP.carFingerprint in LEGACY_CARS:
+      return self.update_legacy(can_parsers)
 
     cp_party = can_parsers[Bus.party]
     cp_ap_party = can_parsers[Bus.ap_party]
@@ -173,10 +189,94 @@ class CarState(CarStateBase):
 
     return ret, fp_ret
 
+  def update_legacy(self, can_parsers):
+    cp_party = can_parsers[Bus.party]
+    cp_ap_party = can_parsers[Bus.ap_party]
+    cp_pt = can_parsers[Bus.pt]
+    cp_ap_pt = can_parsers[Bus.ap_pt]
+    cp_chassis = can_parsers[Bus.chassis]
+    ret = structs.CarState()
+    fp_ret = custom.StarPilotCarState.new_message()
+
+    # Vehicle speed
+    ret.vEgoRaw = cp_chassis.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS
+    ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
+
+    # Gas and brake
+    ret.gasPressed = cp_pt.vl["DI_torque1"]["DI_pedalPos"] > 0
+    ret.brake = 0
+    ret.brakePressed = cp_chassis.vl["BrakeMessage"]["driverBrakeStatus"] != 1
+
+    # Steering wheel and EPAS status
+    epas_status = cp_chassis.vl["EPAS_sysStatus"]
+    self.hands_on_level = epas_status["EPAS_handsOnLevel"]
+    ret.steeringAngleDeg = -epas_status["EPAS_internalSAS"]
+    ret.steeringRateDeg = -cp_chassis.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"]
+    ret.steeringTorque = -epas_status["EPAS_torsionBarTorque"]
+    ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > STEER_THRESHOLD, 5)
+
+    eac_status = self.can_defines["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(epas_status["EPAS_eacStatus"]), None)
+    ret.steerFaultPermanent = eac_status == "EAC_FAULT"
+    ret.steerFaultTemporary = eac_status == "EAC_INHIBITED"
+    eac_error_code = self.can_defines["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(epas_status["EPAS_eacErrorCode"]), None)
+    ret.steeringDisengage = self.hands_on_level >= 3 or (
+      eac_status == "EAC_INHIBITED" and eac_error_code == "EAC_ERROR_HIGH_ANGLE_RATE_SAFETY"
+    )
+
+    # Cruise
+    cruise_state = self.can_defines["DI_state"]["DI_cruiseState"].get(int(cp_chassis.vl["DI_state"]["DI_cruiseState"]), None)
+    speed_units = self.can_defines["DI_state"]["DI_speedUnits"].get(int(cp_chassis.vl["DI_state"]["DI_speedUnits"]), None)
+    cruise_enabled = cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
+    ret.cruiseState.enabled = cruise_enabled
+    if speed_units == "KPH":
+      ret.cruiseState.speed = max(cp_chassis.vl["DI_state"]["DI_hw1CruiseSet"] * CV.KPH_TO_MS, 1e-3)
+    elif speed_units == "MPH":
+      ret.cruiseState.speed = max(cp_chassis.vl["DI_state"]["DI_hw1CruiseSet"] * CV.MPH_TO_MS, 1e-3)
+    ret.cruiseState.available = cruise_state == "STANDBY" or ret.cruiseState.enabled
+    ret.cruiseState.standstill = False
+    ret.standstill = ret.vEgoRaw < 0.1
+    ret.accFaulted = cruise_state == "FAULT"
+
+    # Gear, body state, and safety state
+    ret.gearShifter = GEAR_MAP[self.can_defines["DI_torque2"]["DI_gear"].get(
+      int(cp_chassis.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")]
+
+    doors = ("DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE")
+    ret.doorOpen = any(
+      self.can_defines["GTW_carState"][door].get(int(cp_chassis.vl["GTW_carState"][door]), "OPEN") == "OPEN"
+      for door in doors
+    )
+    ret.leftBlinker = cp_chassis.vl["GTW_carState"]["BC_indicatorLStatus"] == 1
+    ret.rightBlinker = cp_chassis.vl["GTW_carState"]["BC_indicatorRStatus"] == 1
+
+    _ = cp_chassis.vl["SDM1"]
+    _ = cp_chassis.vl["RCM_status"]
+    sd_time = cp_chassis.ts_nanos["SDM1"]["SDM_bcklDrivStatus"]
+    rcm_time = cp_chassis.ts_nanos["RCM_status"]["RCM_buckleDriverStatus"]
+    if sd_time and cp_chassis._last_update_nanos - sd_time <= 1_000_000_000:
+      ret.seatbeltUnlatched = cp_chassis.vl["SDM1"]["SDM_bcklDrivStatus"] != 1
+    elif rcm_time and cp_chassis._last_update_nanos - rcm_time <= 1_000_000_000:
+      ret.seatbeltUnlatched = cp_chassis.vl["RCM_status"]["RCM_buckleDriverStatus"] != 1
+    else:
+      ret.seatbeltUnlatched = True
+
+    ret.stockAeb = cp_ap_pt.vl["DAS_control"]["DAS_aebEvent"] == 1
+    ret.stockLkas = cp_ap_party.vl["DAS_steeringControl"]["DAS_steeringControlType"] == 2
+    self.das_control = copy.copy(cp_ap_pt.vl["DAS_control"])
+    return ret, fp_ret
+
   @staticmethod
   def get_can_parsers(CP):
     if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
       return get_preap_can_parsers(CP)
+    if CP.carFingerprint in LEGACY_CARS:
+      return {
+        Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party),
+        Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party),
+        Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CANBUS.party),
+        Bus.ap_pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CANBUS.autopilot_party),
+        Bus.chassis: CANParser(DBC[CP.carFingerprint][Bus.chassis], [("SDM1", 0), ("RCM_status", 0)], CANBUS.party),
+      }
     return {
       Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party),
       Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party)

opendbc_repo/opendbc/car/tesla/fingerprints.py

@@ -5,6 +5,12 @@ from opendbc.car.tesla.values import CAR
 Ecu = CarParams.Ecu
 
 FW_VERSIONS = {
+  CAR.TESLA_MODEL_S_HW1: {
+    (Ecu.eps, 0x730, None): [
+      b'1016704-00-HAA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\x10\x00A',
+    ],
+  },
   CAR.TESLA_MODEL_3: {
     (Ecu.eps, 0x730, None): [
       b'TeM3_E014p10_0.0.0 (16),E014.17.00',

opendbc_repo/opendbc/car/tesla/interface.py

@@ -1,9 +1,9 @@
-from opendbc.car import get_safety_config, structs
+from opendbc.car import Bus, get_safety_config, structs
 from opendbc.car.interfaces import CarInterfaceBase
 from opendbc.car.tesla.carcontroller import CarController
 from opendbc.car.tesla.carstate import CarState
 from opendbc.car.tesla.radar_interface import RadarInterface
-from opendbc.car.tesla.values import TeslaSafetyFlags, CAR
+from opendbc.car.tesla.values import TeslaSafetyFlags, CAR, DBC, LEGACY_CARS
 from opendbc.car.tesla.preap.interface import get_preap_accel_limits, get_preap_params
 
 
@@ -32,6 +32,20 @@ class CarInterface(CarInterfaceBase):
     if candidate == CAR.TESLA_MODEL_S_PREAP:
       return get_preap_params(ret)
 
+    if candidate in LEGACY_CARS:
+      ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla, TeslaSafetyFlags.FLAG_HW1.value)]
+      ret.steerLimitTimer = 0.4
+      ret.steerActuatorDelay = 0.1
+      ret.steerAtStandstill = True
+      ret.steerControlType = structs.CarParams.SteerControlType.angle
+      ret.radarUnavailable = Bus.radar not in DBC[candidate]
+      ret.alphaLongitudinalAvailable = True
+
+      if alpha_long:
+        ret.openpilotLongitudinalControl = True
+        ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.LONG_CONTROL.value
+      return ret
+
     ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla)]
 
     ret.steerLimitTimer = 0.4

opendbc_repo/opendbc/car/tesla/radar_interface.py

@@ -16,7 +16,7 @@ class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
     super().__init__(CP)
 
-    self.radar_off_can = CP.radarUnavailable or CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP
+    self.radar_off_can = CP.radarUnavailable or Bus.radar not in DBC[CP.carFingerprint]
     self.updated_messages: set[int] = set()
     self.track_id = 0
     self.radar_offset = float(nap_conf.radar_offset) if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP else 0.0

opendbc_repo/opendbc/car/tesla/teslacan_legacy.py

@@ -0,0 +1,53 @@
+from opendbc.car.common.conversions import Conversions as CV
+from opendbc.car.interfaces import V_CRUISE_MAX
+from opendbc.car.tesla.values import CANBUS, CarControllerParams
+
+
+class TeslaCANRaven:
+  """CAN commands used by the legacy Model S/X powertrain and EPAS buses."""
+
+  def __init__(self, packers):
+    self.packers = packers
+    self.CCP = CarControllerParams
+    self.jerk_upper = self.CCP.JERK_LIMIT_MAX
+    self.jerk_lower = self.CCP.JERK_LIMIT_MIN
+
+  @staticmethod
+  def checksum(msg_id, dat):
+    return ((msg_id & 0xFF) + ((msg_id >> 8) & 0xFF) + sum(dat)) & 0xFF
+
+  def create_steering_control(self, counter, angle, enabled):
+    values = {
+      "DAS_steeringControlCounter": counter,
+      "DAS_steeringAngleRequest": -angle,
+      "DAS_steeringHapticRequest": 0,
+      "DAS_steeringControlType": 1 if enabled else 0,
+    }
+    data = self.packers[CANBUS.party].make_can_msg("DAS_steeringControl", CANBUS.party, values)[1]
+    values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3])
+    return self.packers[CANBUS.party].make_can_msg("DAS_steeringControl", CANBUS.party, values)
+
+  def create_longitudinal_command(self, acc_state, accel, counter, v_ego, active, gas_pressed):
+    set_speed = max(v_ego * CV.MS_TO_KPH, 0)
+    if active:
+      set_speed = 0 if accel < 0 else V_CRUISE_MAX
+
+    if gas_pressed:
+      self.jerk_upper = self.jerk_lower = 0.0
+    else:
+      self.jerk_lower = max(self.jerk_lower - self.CCP.JERK_RAMP_RATE, self.CCP.JERK_LIMIT_MIN)
+      self.jerk_upper = min(self.jerk_upper + self.CCP.JERK_RAMP_RATE, self.CCP.JERK_LIMIT_MAX)
+
+    values = {
+      "DAS_setSpeed": set_speed,
+      "DAS_accState": acc_state,
+      "DAS_aebEvent": 0,
+      "DAS_jerkMin": self.jerk_lower,
+      "DAS_jerkMax": self.jerk_upper,
+      "DAS_accelMin": accel,
+      "DAS_accelMax": max(accel, 0),
+      "DAS_controlCounter": counter,
+    }
+    data = self.packers[CANBUS.party].make_can_msg("DAS_control", CANBUS.party, values)[1]
+    values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7])
+    return self.packers[CANBUS.party].make_can_msg("DAS_control", CANBUS.party, values)

opendbc_repo/opendbc/car/tesla/tests/replay_hw1_route.py

@@ -0,0 +1,148 @@
+#!/usr/bin/env python3
+"""Offline AP1/HW1 CAN, radar, controller, and panda-safety replay.
+
+Usage: PYTHONPATH=. python -m opendbc.car.tesla.tests.replay_hw1_route PATH_TO_RLOGS
+
+The supplied Pre-AP recording contains stock AP commands copied to bus 0 while
+ELM327/old firmware forwarded traffic. The counterfactual run skips those copies:
+the HW1 safety mode blocks stock 0x488/0x2b9 forwarding on bus 2. This is NOT a
+physical HW1 drive; it cannot validate engagement or steering actuation on-car.
+"""
+
+import argparse
+from collections import Counter
+from pathlib import Path
+
+from cereal import custom
+from openpilot.tools.lib.logreader import LogReader
+from opendbc.car import Bus, structs
+from opendbc.car.tesla.carcontroller import CarController
+from opendbc.car.tesla.carstate import CarState
+from opendbc.car.tesla.interface import CarInterface
+from opendbc.car.tesla.radar_interface import RadarInterface
+from opendbc.car.tesla.values import CAR, DBC, TeslaSafetyFlags
+from opendbc.safety.tests.libsafety import libsafety_py
+
+
+def replay(paths: list[Path], simulate_active: bool = False):
+  fp = {0: {0x201: 5}, 1: {}, 2: {}}
+  cp = CarInterface.get_params(CAR.TESLA_MODEL_S_HW1, fp, [], True, False, False, None)
+  assert cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.tesla
+  assert cp.safetyConfigs[0].safetyParam == TeslaSafetyFlags.FLAG_HW1.value | TeslaSafetyFlags.LONG_CONTROL.value
+  safety = libsafety_py.libsafety
+  assert safety.set_safety_hooks(int(structs.CarParams.SafetyModel.tesla), cp.safetyConfigs[0].safetyParam) == 0
+  safety.init_tests()
+
+  parsers = CarState.get_can_parsers(cp)
+  cs = CarState(cp, custom.StarPilotCarParams.new_message())
+  controller = CarController(DBC[CAR.TESLA_MODEL_S_HW1], cp)
+  active_controller = CarController(DBC[CAR.TESLA_MODEL_S_HW1], cp) if simulate_active else None
+  radar = RadarInterface(cp)
+  stats = Counter()
+  first_rejected = []
+  active_rejected = []
+  last_ap_command: dict[tuple[int, bytes], int] = {}
+  suppressed_examples = []
+
+  for path in paths:
+    for event in LogReader(str(path)):
+      if event.which() != "can":
+        continue
+      t = event.logMonoTime
+      frames = [(x.address, bytes(x.dat), x.src) for x in event.can]
+      stock_in_event = {(a, d) for a, d, b in frames if b == 2 and a in (0x488, 0x2b9)}
+      for a, d, b in frames:
+        if b == 2 and a in (0x488, 0x2b9):
+          last_ap_command[(a, d)] = t
+        if b == 0 and a in (0x488, 0x2b9):
+          seen = last_ap_command.get((a, d), -1)
+          if (a, d) in stock_in_event or (0 <= t - seen < 250_000_000):
+            stats["suppressed_bus0_stock_copies"] += 1
+            continue
+          stats["unmatched_bus0_stock_commands"] += 1
+          if len(suppressed_examples) < 5:
+            suppressed_examples.append((path.name, t, hex(a), d.hex()))
+        if b < 128:
+          stats["physical_rx"] += 1
+          if not safety.safety_rx_hook(libsafety_py.make_CANPacket(a, b, d)):
+            stats["rx_rejected"] += 1
+          if b == 2 and a in (0x488, 0x2b9):
+            stats["stock_forward_blocked"] += safety.safety_fwd_hook(b, a) == -1
+
+      safety.set_timer((t // 1000) & 0xffffffff)
+      safety.safety_tick_current_safety_config()
+      stats["safety_invalid_ticks"] += not safety.safety_config_valid()
+      stats["relay_malfunction_ticks"] += safety.get_relay_malfunction()
+      stats["controls_allowed_ticks"] += safety.get_controls_allowed()
+
+      batch = [(t, frames)]
+      for parser in parsers.values():
+        parser.update(batch)
+        stats["invalid_car_parser_ticks"] += not parser.can_valid
+      out, _ = cs.update(parsers, None)
+      cs.out = out
+      stats["carstate_faulted_ticks"] += out.accFaulted
+      stats["seatbelt_unlatched_ticks"] += out.seatbeltUnlatched
+      stats["steering_inhibited_ticks"] += out.steerFaultTemporary
+      stats["cruise_engaged_ticks"] += out.cruiseState.enabled
+
+      radar_data = radar.update(batch)
+      if radar_data is not None:
+        stats["radar_updates"] += 1
+        stats["radar_points"] += len(radar_data.points)
+        stats["radar_error_updates"] += radar_data.errors.canError or radar_data.errors.radarFault
+
+      cc = structs.CarControl.new_message()
+      cc.actuators.steeringAngleDeg = out.steeringAngleDeg
+      cc.actuators.accel = 0.
+      # Do not fabricate engagement on the actual faulted/standby route.
+      _, sends = controller.update(cc.as_reader(), cs, t, None)
+      for a, d, b in sends:
+        stats["generated_tx"] += 1
+        stats[f"generated_{hex(a)}"] += 1
+        if not safety.safety_tx_hook(libsafety_py.make_CANPacket(a, b, d)):
+          stats["tx_rejected"] += 1
+          if len(first_rejected) < 5:
+            first_rejected.append((path.name, t, hex(a), d.hex(), out.steeringAngleDeg, safety.get_relay_malfunction()))
+
+      if active_controller is not None:
+        # A synthetic gate test only. This recording never engaged cruise, so
+        # enabling controls here does NOT represent an actual car-state transition.
+        eligible = (not (out.steerFaultTemporary or out.steerFaultPermanent or out.steeringDisengage or out.accFaulted or
+                         out.gasPressed or out.brakePressed or out.stockAeb or out.stockLkas) and out.vEgoRaw > 2.)
+        simulated = structs.CarControl.new_message()
+        simulated.latActive = eligible
+        simulated.longActive = eligible
+        simulated.actuators.steeringAngleDeg = out.steeringAngleDeg
+        simulated.actuators.accel = 0.5 if eligible else 0.
+        _, active_sends = active_controller.update(simulated.as_reader(), cs, t, None)
+        if eligible:
+          stats["simulated_eligible_ticks"] += 1
+          safety.set_controls_allowed(True)
+          for a, d, b in active_sends:
+            stats["simulated_tx"] += 1
+            stats[f"simulated_{hex(a)}"] += 1
+            if not safety.safety_tx_hook(libsafety_py.make_CANPacket(a, b, d)):
+              stats["simulated_tx_rejected"] += 1
+              if len(active_rejected) < 5:
+                active_rejected.append((path.name, t, hex(a), d.hex(), out.steeringAngleDeg, out.vEgoRaw))
+          safety.set_controls_allowed(False)
+      stats["can_events"] += 1
+    print(f"{path.name}: {dict(stats)}", flush=True)
+
+  print(f"unmatched bus-0 command examples: {suppressed_examples}")
+  print(f"rejected TX examples: {first_rejected}")
+  print(f"rejected synthetic-active TX examples: {active_rejected}")
+  print(f"final: {dict(stats)}")
+  return stats
+
+
+if __name__ == "__main__":
+  argp = argparse.ArgumentParser(description=__doc__)
+  argp.add_argument("rlogs", type=Path, help="directory containing segment rlog.zst files")
+  argp.add_argument("--simulate-active", action="store_true", help="force safety engagement only on healthy standby samples")
+  args = argp.parse_args()
+  files = sorted(args.rlogs.glob("*.rlog.zst"))
+  if not files:
+    argp.error("no *.rlog.zst files found")
+  replay(files, args.simulate_active)

opendbc_repo/opendbc/car/tesla/tests/test_hw1.py

@@ -0,0 +1,132 @@
+import pytest
+
+from cereal import custom
+
+from opendbc.can import CANPacker, CANParser
+from opendbc.car import Bus, structs
+from opendbc.car.fw_versions import match_fw_to_car
+from opendbc.car.tesla.carcontroller import CarController
+from opendbc.car.tesla.carstate import CarState
+from opendbc.car.tesla.fingerprints import FW_VERSIONS
+from opendbc.car.tesla.interface import CarInterface
+from opendbc.car.tesla.teslacan_legacy import TeslaCANRaven
+from opendbc.car.tesla.values import CANBUS, CAR, DBC, TeslaSafetyFlags
+
+
+def test_hw1_is_distinct_from_preap_and_does_not_change_can_buses():
+  preap = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP)
+  hw1 = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1)
+
+  assert preap.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.teslaPreAP
+  assert hw1.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.tesla
+  assert hw1.safetyConfigs[0].safetyParam == TeslaSafetyFlags.FLAG_HW1.value
+  assert CANBUS.party == 0 and CANBUS.autopilot_party == 2
+  assert CarState.get_can_parsers(hw1)[Bus.ap_pt].bus == 2
+  assert CarState.get_can_parsers(hw1)[Bus.chassis].message_states[0x211].ignore_alive
+  assert CarState.get_can_parsers(preap)[Bus.party].bus == 0
+
+
+def test_hw1_requires_explicit_alpha_long_for_acceleration():
+  ret = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1)
+  hw1 = CarInterface._get_params(ret, CAR.TESLA_MODEL_S_HW1, {0: {0x201: 5}}, [], True, False, False)
+  assert hw1.openpilotLongitudinalControl
+  assert hw1.safetyConfigs[0].safetyParam == TeslaSafetyFlags.FLAG_HW1.value | TeslaSafetyFlags.LONG_CONTROL.value
+
+
+def test_ap1_eps_fw_matches_hw1_without_matching_preap():
+  version = FW_VERSIONS[CAR.TESLA_MODEL_S_HW1][(structs.CarParams.Ecu.eps, 0x730, None)][0]
+  fw = structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.eps, address=0x730, brand="tesla", fwVersion=version)
+  exact, candidates = match_fw_to_car([fw], "", log=False)
+  assert exact and candidates == {CAR.TESLA_MODEL_S_HW1}
+
+
+def test_hw1_display_and_cruise_bytes_do_not_change_preap_signals():
+  # Captured bus-0 DI_state (0x368) from the AP1 route: display 9 MPH, set speed 10 MPH.
+  parser = CANParser("tesla_can", [(0x368, 0)], 0)
+  parser.message_states[0x368].ignore_counter = True
+  frames = [(0x368, bytes.fromhex("84185e3009980a2d"), 0)]
+  parser.update([(1_000_000_000, frames)])
+  parser.update([(2_000_000_000, frames)])
+  state = parser.vl["DI_state"]
+  assert state["DI_hw1DigitalSpeed"] == 9
+  assert state["DI_hw1CruiseSet"] == 10
+  assert state["DI_digitalSpeed"] == 10
+  assert state["DI_cruiseSet"] != 10
+
+
+def test_hw1_packer_emits_bus_zero_with_matching_checksums():
+  packer = CANPacker(DBC[CAR.TESLA_MODEL_S_HW1][Bus.party])
+  tesla_can = TeslaCANRaven({CANBUS.party: packer})
+  for msg, expected_addr, checksum_index in (
+    (tesla_can.create_steering_control(0, 0, False), 0x488, 3),
+    (tesla_can.create_longitudinal_command(13, 0, 0, 10, False, False), 0x2b9, 7),
+  ):
+    addr, data, bus = msg
+    assert addr == expected_addr and bus == 0
+    assert data[checksum_index] == TeslaCANRaven.checksum(addr, data[:checksum_index])
+
+
+def test_hw1_cancel_clears_acceleration_and_does_not_request_max_speed():
+  cp = CarInterface._get_params(CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1),
+                                CAR.TESLA_MODEL_S_HW1, {0: {}}, [], True, False, False)
+  controller = CarController(DBC[CAR.TESLA_MODEL_S_HW1], cp)
+  state = CarState(cp, custom.StarPilotCarParams.new_message())
+  state.out.vEgo = 10.
+  cc = structs.CarControl.new_message()
+  cc.longActive = True
+  cc.cruiseControl.cancel = True
+  cc.actuators.accel = 2.
+  _, sends = controller.update(cc.as_reader(), state, 0, None)
+  _, data, bus = next(msg for msg in sends if msg[0] == 0x2b9)
+  assert bus == 0
+  parser = CANParser("tesla_can", [(0x2b9, 0)], 0)
+  parser.update([(1_000_000_000, [(0x2b9, data, bus)])])
+  decoded = parser.vl["DAS_control"]
+  assert decoded["DAS_accState"] == 13
+  assert decoded["DAS_accelMin"] == pytest.approx(0, abs=0.05)
+  assert decoded["DAS_accelMax"] == pytest.approx(0, abs=0.05)
+  assert decoded["DAS_setSpeed"] != 200
+
+
+def test_hw1_carstate_uses_ap1_powertrain_and_chassis():
+  cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1)
+  parsers = CarState.get_can_parsers(cp)
+  frames = [
+    (0x155, bytes.fromhex("000000000005e308"), 0),  # ESP speed 15.07 kph
+    (0x368, bytes.fromhex("84185e3009980a2d"), 0),
+    (0x201, bytes.fromhex("5444008df2"), 0),
+  ]
+  for parser in parsers.values():
+    for addr in (0x155, 0x368, 0x201):
+      _ = parser.vl[addr]
+      parser.message_states[addr].ignore_counter = True
+      parser.message_states[addr].ignore_checksum = True
+    parser.update([(1_000_000_000, frames)])
+    parser.update([(2_000_000_000, frames)])
+  state = CarState(cp, custom.StarPilotCarParams.new_message())
+  ret, _ = state.update(parsers, None)
+  assert not ret.cruiseState.enabled
+  assert ret.cruiseState.speed == pytest.approx(10 * 0.44704)
+  assert ret.vEgoRaw == pytest.approx(15.07 / 3.6)
+  assert not ret.seatbeltUnlatched
+
+  # A stale belt frame cannot allow an engagement indefinitely.
+  for parser in parsers.values():
+    parser.update([(4_000_000_000, [])])
+  ret, _ = state.update(parsers, None)
+  assert ret.seatbeltUnlatched
+
+
+def test_hw1_can_use_rcm_buckle_when_sdm1_is_absent():
+  cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1)
+  parsers = CarState.get_can_parsers(cp)
+  packer = CANPacker(DBC[CAR.TESLA_MODEL_S_HW1][Bus.chassis])
+  addr, data, bus = packer.make_can_msg("RCM_status", 0, {"RCM_buckleDriverStatus": 1})
+  assert addr == 0x211 and bus == 0
+  frames = [(addr, data, bus)]
+  for parser in parsers.values():
+    _ = parser.vl["RCM_status"]
+    parser.update([(1_000_000_000, frames)])
+  state = CarState(cp, custom.StarPilotCarParams.new_message())
+  out, _ = state.update(parsers, None)
+  assert not out.seatbeltUnlatched

opendbc_repo/opendbc/car/tesla/values.py

@@ -70,6 +70,16 @@ class CAR(Platforms):
       Bus.radar: 'tesla_radar_bosch_generated',
     },
   )
+  TESLA_MODEL_S_HW1 = TeslaPlatformConfig(
+    [CarDocs("Tesla Model S (with HW1) 2014-16", "All", support_type=SupportType.COMMUNITY, support_link="#community")],
+    CarSpecs(mass=2100., wheelbase=2.960, steerRatio=15.0),
+    {
+      Bus.chassis: 'tesla_can',
+      Bus.party: 'tesla_can',
+      Bus.pt: 'tesla_can',
+      Bus.radar: 'tesla_radar_bosch_generated',
+    },
+  )
 
 
 FW_QUERY_CONFIG = FwQueryConfig(
@@ -125,10 +135,14 @@ class CarControllerParams:
   ACCEL_MAX = 2.0    # m/s^2
   ACCEL_MIN = -3.48  # m/s^2
   JERK_LIMIT_MAX = 4.9  # m/s^3, ACC faults at 5.0
+  JERK_LIMIT_MIN = -4.9  # m/s^3, ACC faults at 5.0
+  JERK_RAMP_RATE = JERK_LIMIT_MAX * 0.002
 
 
 class TeslaSafetyFlags(IntFlag):
   LONG_CONTROL = 1
+  FLAG_EXTERNAL_PANDA = 4
+  FLAG_HW1 = 8
   COOP_STEERING = 256
 
 
@@ -157,5 +171,7 @@ class CruiseButtons:
 
 DBC = CAR.create_dbc_map()
 
+LEGACY_CARS = (CAR.TESLA_MODEL_S_HW1,)
+
 STEER_THRESHOLD = 1
 STEER_DISENGAGE_THRESHOLD = 5.0

opendbc_repo/opendbc/car/torque_data/override.toml

@@ -26,6 +26,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 "TESLA_MODEL_3" = [nan, 2.5, nan]
 "TESLA_MODEL_Y" = [nan, 2.5, nan]
 "TESLA_MODEL_X" = [nan, 2.5, nan]
+"TESLA_MODEL_S_HW1" = [nan, 2.5, nan]
 
 # Guess
 "VOLKSWAGEN_ID3_MK1" = [nan, 2.5, nan]

opendbc_repo/opendbc/car/toyota/carcontroller.py

@@ -47,7 +47,6 @@ TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES = 100
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 18  # tx control frames needed before torque can be cut
-TOYOTA_COROLLA_TSS2_MAX_STEER_RATE_FRAMES = 8
 
 # EPS allows user torque above threshold for 50 frames before permanently faulting
 MAX_USER_TORQUE = 500
@@ -78,9 +77,13 @@ def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
   ) or highlander_sdsu)
 
 
-def get_steer_rate_limit_frames(car_fingerprint) -> int:
-  return (TOYOTA_COROLLA_TSS2_MAX_STEER_RATE_FRAMES
-          if car_fingerprint == CAR.TOYOTA_COROLLA_TSS2 else MAX_STEER_RATE_FRAMES)
+def apply_toyota_corolla_steer_rate_guard(car_fingerprint, steering_rate_deg: float, lat_active: bool,
+                                           apply_torque: int, apply_steer_req: bool) -> tuple[int, bool]:
+  if (car_fingerprint == CAR.TOYOTA_COROLLA_TSS2 and lat_active and
+      abs(steering_rate_deg) >= MAX_STEER_RATE):
+    return 0, True
+
+  return apply_torque, apply_steer_req
 
 
 def supports_toyota_auto_hold(CP, auto_hold_enabled: bool) -> bool:
@@ -250,7 +253,6 @@ class CarController(CarControllerBase):
     self.standstill_req = False
     self.permit_braking = True
     self.steer_rate_counter = 0
-    self.steer_rate_limit_frames = get_steer_rate_limit_frames(self.CP.carFingerprint)
     self.distance_button = 0
 
     # *** start long control state ***
@@ -370,7 +372,11 @@ class CarController(CarControllerBase):
     # >100 degree/sec steering fault prevention
     self.steer_rate_counter, apply_steer_req = common_fault_avoidance(
       abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE, lat_active,
-      self.steer_rate_counter, self.steer_rate_limit_frames,
+      self.steer_rate_counter, MAX_STEER_RATE_FRAMES,
+    )
+
+    apply_torque, apply_steer_req = apply_toyota_corolla_steer_rate_guard(
+      self.CP.carFingerprint, CS.out.steeringRateDeg, lat_active, apply_torque, apply_steer_req,
     )
 
     if not lat_active:

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

@@ -11,7 +11,7 @@ from opendbc.car.toyota import toyotacan
 from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \
                                              get_prius_positive_feedforward_scale, \
                                              get_rav4_interceptor_pedal_scale, \
-                                             get_steer_rate_limit_frames, \
+                                             apply_toyota_corolla_steer_rate_guard, \
                                              limit_interceptor_pcm_accel, \
                                              limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \
                                              limit_prius_stopping_accel, should_bypass_toyota_long_pid, supports_toyota_auto_hold, \
@@ -735,9 +735,15 @@ class TestToyotaFingerprint:
 
 
 class TestToyotaCarController:
-  def test_corolla_tss2_uses_early_steer_rate_fault_guard(self):
-    assert get_steer_rate_limit_frames(CAR.TOYOTA_COROLLA_TSS2) == 8
-    assert get_steer_rate_limit_frames(CAR.TOYOTA_RAV4_TSS2) == 18
+  def test_corolla_tss2_cuts_torque_but_keeps_lka_request_at_high_steer_rate(self):
+    assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_COROLLA_TSS2, 100.0, True, 250, True) == (0, True)
+    assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_COROLLA_TSS2, 120.0, True, 0, False) == (0, True)
+
+  def test_corolla_tss2_steer_rate_guard_leaves_normal_rate_unchanged(self):
+    assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_COROLLA_TSS2, 99.9, True, 250, True) == (250, True)
+
+  def test_corolla_tss2_steer_rate_guard_is_corolla_only(self):
+    assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_RAV4_TSS2, 120.0, True, 250, True) == (250, True)
 
   @staticmethod
   def _make_controller(*, standstill_req=False, last_standstill=False):

opendbc_repo/opendbc/dbc/hyundai_kia_ray_pedal.dbc

@@ -0,0 +1,23 @@
+VERSION ""
+
+NS_ :
+BS_:
+BU_: INTERCEPTOR NEO
+
+BO_ 512 GAS_COMMAND: 6 NEO
+ SG_ GAS_COMMAND : 7|16@0+ (0.672,-177.408) [0|255] "" INTERCEPTOR
+ SG_ GAS_COMMAND2 : 23|16@0+ (0.332,-165.004) [0|255] "" INTERCEPTOR
+ SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR
+ SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" INTERCEPTOR
+ SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" INTERCEPTOR
+
+BO_ 513 GAS_SENSOR: 6 INTERCEPTOR
+ SG_ INTERCEPTOR_GAS : 7|16@0+ (0.672,-177.408) [0|255] "" NEO
+ SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.332,-165.004) [0|255] "" NEO
+ SG_ STATE : 39|4@0+ (1,0) [0|15] "" NEO
+ SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" NEO
+ SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" NEO
+
+VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ;
+
+CM_ "Kia Ray EV comma pedal uses the standard 0x200/0x201 rolling counter and CRC8 protocol. Channel scaling is Ray-only, estimated from the September 15 route: sensor rest raw 264/497, slopes about 3.79/7.68 raw counts per native E_EMS11 pedal unit, and 100 native units mapped to 255 comma pedal command units. Confirm against installed Ray firmware before increasing the command cap.";

opendbc_repo/opendbc/dbc/tesla_can.dbc

@@ -217,6 +217,9 @@ BO_ 513 SDM1: 5 GTW
  SG_ SDM_bcklPassStatus : 3|2@0+ (1,0) [0|3] "" NEO
  SG_ SDM_bcklDrivStatus : 5|2@0+ (1,0) [0|3] "" NEO
 
+BO_ 529 RCM_status: 8 RCM
+ SG_ RCM_buckleDriverStatus : 15|2@0+ (1,0) [0|3] "" GTW,OCS,DAS
+
 BO_ 532 EPB_epasControl: 3 EPB
  SG_ EPB_epasControlChecksum : 23|8@0+ (1,0) [0|255] ""  NEO,EPAS
  SG_ EPB_epasControlCounter : 11|4@0+ (1,0) [0|15] ""  NEO,EPAS
@@ -256,9 +259,11 @@ BO_ 872 DI_state: 8 DI
  SG_ DI_immobilizerState : 28|3@1+ (1,0) [0|0] ""  NEO
  SG_ DI_speedUnits : 31|1@1+ (1,0) [0|1] ""  NEO
  SG_ DI_cruiseSet : 32|9@1+ (0.5,0) [0|255.5] "speed"  NEO
+ SG_ DI_hw1DigitalSpeed : 32|8@1+ (1,0) [0|250] "speed" NEO
  SG_ DI_aebState : 41|3@1+ (1,0) [0|0] ""  NEO
  SG_ DI_stateCounter : 44|4@1+ (1,0) [0|0] ""  NEO
  SG_ DI_digitalSpeed : 48|8@1+ (1,0) [0|250] ""  NEO
+ SG_ DI_hw1CruiseSet : 48|8@1+ (1,0) [0|250] "speed" NEO
  SG_ DI_stateChecksum : 56|8@1+ (1,0) [0|0] ""  NEO
 
 BO_ 109 SBW_RQ_SCCM: 4 STW
@@ -906,4 +911,3 @@ VAL_ 1001 DAS_turnIndicatorRequestReason 6 "DAS_ACTIVE_COMMANDED_LANE_CHANGE" 5
 VAL_ 1160 DAS_steeringAngleRequest 16384 "ZERO_ANGLE" ;
 VAL_ 1160 DAS_steeringControlType 1 "ANGLE_CONTROL" 3 "DISABLED" 0 "NONE" 2 "RESERVED" ;
 VAL_ 1160 DAS_steeringHapticRequest 1 "ACTIVE" 0 "IDLE" ;
-

opendbc_repo/opendbc/safety/declarations.h

@@ -384,3 +384,4 @@ extern const safety_hooks rivian_hooks;
 extern const safety_hooks psa_hooks;
 extern const safety_hooks volvo_hooks;
 extern const safety_hooks tesla_preap_hooks;
+extern const safety_hooks tesla_legacy_hooks;

opendbc_repo/opendbc/safety/modes/hyundai.h

@@ -67,6 +67,9 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
 #define HYUNDAI_NON_SCC_EV_ADDR_CHECK \
   {.msg = {{0x592U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
 
+#define HYUNDAI_RAY_PEDAL_ADDR_CHECK \
+  {.msg = {{0x201U, 0, 6, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
+
 static const CanMsg HYUNDAI_TX_MSGS[] = {
   HYUNDAI_COMMON_TX_MSGS(0, false)
 };
@@ -75,6 +78,11 @@ static const CanMsg HYUNDAI_REFRESH_TX_MSGS[] = {
   HYUNDAI_COMMON_TX_MSGS(0, true)
 };
 
+static const CanMsg HYUNDAI_RAY_PEDAL_TX_MSGS[] = {
+  HYUNDAI_COMMON_TX_MSGS(0, true)
+  {0x200, 0, 6, .check_relay = false},  // comma pedal only, not Hyundai EMS20
+};
+
 static const CanMsg HYUNDAI_LONG_TX_MSGS[] = {
   HYUNDAI_LONG_COMMON_TX_MSGS(0, false)
   {0x38D, 0, 8, .check_relay = false}, // FCA11 Bus 0
@@ -90,6 +98,7 @@ static const CanMsg HYUNDAI_LONG_REFRESH_TX_MSGS[] = {
 };
 
 static bool hyundai_legacy = false;
+static bool hyundai_ray_pedal = false;
 static bool hyundai_can_canfd_blended_hda2 = false;
 static bool hyundai_acc_main_on_rx_prev = false;
 
@@ -120,6 +129,8 @@ static uint8_t hyundai_get_counter(const CANPacket_t *msg) {
     cnt = byte_421 & 0xFU;
   } else if (msg->addr == 0x4F1U) {
     cnt = (msg->data[3] >> 4) & 0xFU;
+  } else if (hyundai_ray_pedal && (msg->addr == 0x201U)) {
+    cnt = msg->data[4] & 0xFU;
   } else {
   }
   return cnt;
@@ -136,11 +147,24 @@ static uint32_t hyundai_get_checksum(const CANPacket_t *msg) {
     chksum = msg->data[6] & 0xFU;
   } else if (msg->addr == 0x421U) {
     chksum = hyundai_can_canfd_blended ? msg->data[0] : msg->data[7] >> 4;
+  } else if (hyundai_ray_pedal && (msg->addr == 0x201U)) {
+    chksum = msg->data[5];
   } else {
   }
   return chksum;
 }
 
+static uint8_t hyundai_ray_pedal_checksum(const CANPacket_t *msg) {
+  uint8_t crc = 0xFFU;
+  for (int i = 4; i >= 0; i--) {
+    crc ^= msg->data[i];
+    for (int j = 0; j < 8; j++) {
+      crc = (crc & 0x80U) ? (uint8_t)((crc << 1U) ^ 0xD5U) : (uint8_t)(crc << 1U);
+    }
+  }
+  return crc;
+}
+
 static void hyundai_rx_all_hook(const CANPacket_t *msg) {
   if ((msg->addr == 0x53EU) && (msg->bus == 2U) && (GET_LEN(msg) == 6U)) {
     hyundai_has_lkas12 = true;
@@ -148,6 +172,10 @@ static void hyundai_rx_all_hook(const CANPacket_t *msg) {
 }
 
 static uint32_t hyundai_compute_checksum(const CANPacket_t *msg) {
+  if (hyundai_ray_pedal && (msg->addr == 0x201U)) {
+    return hyundai_ray_pedal_checksum(msg);
+  }
+
   uint8_t chksum = 0;
   if (msg->addr == 0x386U) {
     // count the bits
@@ -291,6 +319,22 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) {
 
   bool tx = true;
 
+  if (hyundai_ray_pedal && (msg->addr == 0x200U)) {
+    const uint16_t track1 = ((uint16_t)msg->data[0] << 8U) | msg->data[1];
+    const uint16_t track2 = ((uint16_t)msg->data[2] << 8U) | msg->data[3];
+    const bool enabled = (msg->data[4] & 0x80U) != 0U;
+    const int expected_track2 = 497 + (2 * ((int)track1 - 264));
+    if ((msg->data[4] & 0x70U) != 0U ||
+        (msg->data[5] != hyundai_ray_pedal_checksum(msg)) ||
+        (enabled && (track1 < 264U || track1 > 397U || track2 < 497U || track2 > 766U ||
+                     SAFETY_ABS((int)track2 - expected_track2) > 40)) ||
+        (!enabled && ((track1 != 0U) || (track2 != 0U))) ||
+        longitudinal_interceptor_checks(msg) ||
+        (enabled && (!get_longitudinal_allowed() || brake_pressed_prev))) {
+      tx = false;
+    }
+  }
+
   if ((msg->addr == 0x53EU) && !hyundai_has_lkas12) {
     tx = false;
   }
@@ -457,6 +501,10 @@ static safety_config hyundai_init(uint16_t param) {
   };
 
   hyundai_common_init(param);
+  hyundai_ray_pedal = (param & (uint16_t)~(32U | 128U | 2048U)) == 0x9405U;
+  if (hyundai_ray_pedal) {
+    hyundai_longitudinal = true;  // button engagement; no Hyundai SCC TX
+  }
   hyundai_legacy = false;
   hyundai_can_canfd_blended_hda2 = hyundai_can_canfd_blended && hyundai_canfd_lka_steering;
   hyundai_aol_main_lkas_sync = GET_FLAG(param, 32U);
@@ -467,6 +515,17 @@ static safety_config hyundai_init(uint16_t param) {
   }
 
   safety_config ret;
+  if (hyundai_ray_pedal) {
+    static RxCheck hyundai_ray_pedal_rx_checks[] = {
+      HYUNDAI_COMMON_RX_CHECKS(false)
+      HYUNDAI_NON_SCC_EV_ADDR_CHECK
+      HYUNDAI_LDA_BUTTON_ADDR_CHECK
+      HYUNDAI_RAY_PEDAL_ADDR_CHECK
+    };
+    SET_RX_CHECKS(hyundai_ray_pedal_rx_checks, ret);
+    SET_TX_MSGS(HYUNDAI_RAY_PEDAL_TX_MSGS, ret);
+    return ret;
+  }
   if (hyundai_longitudinal) {
     // Use CLU11 (buttons) to manage controls allowed instead of SCC cruise state
     static RxCheck hyundai_long_rx_checks[] = {
@@ -696,6 +755,7 @@ static safety_config hyundai_legacy_init(uint16_t param) {
 
   hyundai_common_init(param);
   hyundai_legacy = true;
+  hyundai_ray_pedal = false;
   hyundai_can_canfd_blended_hda2 = false;
   hyundai_camera_scc = false;
   hyundai_can_refresh_msgs = false;

opendbc_repo/opendbc/safety/modes/tesla_legacy.h

@@ -0,0 +1,241 @@
+#pragma once
+
+#include "opendbc/safety/declarations.h"
+
+#define TESLA_LEGACY_FLAG_HW1 8U
+
+static bool tesla_external_panda = false;
+static bool tesla_hw1 = false;
+static bool tesla_hw2 = false;
+static bool tesla_hw3 = false;
+static bool tesla_legacy_longitudinal = false;
+
+static int chassis_bus = 0U;
+static int das_control_msg = 0x2bfU;
+static int di_torque1_msg = 0x106U;
+
+static bool tesla_legacy_stock_aeb = false;
+static bool tesla_legacy_stock_lkas = false;
+static bool tesla_legacy_stock_lkas_prev = false;
+
+static void tesla_legacy_rx_hook(const CANPacket_t *msg) {
+  // EPAS_sysStatus: steering angle, driver hands, and EAC status.
+  if (!tesla_external_panda && (msg->bus == 0U) && (msg->addr == 0x370U)) {
+    const int angle_meas_new = (((msg->data[4] & 0x3FU) << 8) | msg->data[5]) - 8192U;
+    update_sample(&angle_meas, angle_meas_new);
+
+    const int hands_on_level = msg->data[4] >> 6;
+    const int eac_status = msg->data[6] >> 5;
+    const int eac_error_code = msg->data[2] >> 4;
+    steering_disengage = (hands_on_level >= 3) || ((eac_status == 0) && (eac_error_code == 9));
+  }
+
+  // ESP_B: ESP_vehicleSpeed.
+  if (!tesla_external_panda && (msg->bus == (unsigned)chassis_bus) && (msg->addr == 0x155U)) {
+    const float speed = ((msg->data[6] | (msg->data[5] << 8)) * 0.01) * KPH_TO_MS;
+    UPDATE_VEHICLE_SPEED(speed);
+  }
+
+  // DI_torque1: pedal position. HW1 uses the 0x108 message variant.
+  if ((tesla_external_panda || tesla_hw1) && (msg->bus == 0U) && (msg->addr == di_torque1_msg)) {
+    gas_pressed = msg->data[6] != 0U;
+  }
+
+  if (((tesla_external_panda) && (msg->bus == 0U) && (msg->addr == 0x1f8U)) ||
+      ((!tesla_external_panda) && (msg->bus == (unsigned)chassis_bus) && (msg->addr == 0x20aU))) {
+    brake_pressed = (((msg->data[0] & 0x0CU) >> 2) != 1U);
+  }
+
+  // DI_state: cruise state.
+  if (((tesla_external_panda) && (msg->bus == 0U) && (msg->addr == 0x256U)) ||
+      ((!tesla_external_panda) && (msg->bus == (unsigned)chassis_bus) && (msg->addr == 0x368U))) {
+    const int cruise_state = (msg->data[1] >> 4) & 0x07U;
+    const bool cruise_engaged = (cruise_state == 2) || (cruise_state == 3) || (cruise_state == 4) ||
+                                (cruise_state == 6) || (cruise_state == 7);
+    vehicle_moving = cruise_state != 3;
+    pcm_cruise_check(cruise_engaged);
+  }
+
+  if (msg->bus == 2U) {
+    if ((tesla_external_panda || tesla_hw1) && msg->addr == das_control_msg) {
+      tesla_legacy_stock_aeb = (msg->data[2] & 0x03U) == 1U;
+    }
+
+    if (!tesla_external_panda && msg->addr == 0x488U) {
+      const int steering_control_type = msg->data[2] >> 6;
+      const bool stock_lkas_now = steering_control_type == 2;
+      if (stock_lkas_now && !tesla_legacy_stock_lkas_prev && !controls_allowed) {
+        tesla_legacy_stock_lkas = true;
+      }
+      if (!stock_lkas_now) {
+        tesla_legacy_stock_lkas = false;
+      }
+      tesla_legacy_stock_lkas_prev = stock_lkas_now;
+    }
+  }
+}
+
+static bool tesla_legacy_tx_hook(const CANPacket_t *msg) {
+  const AngleSteeringLimits TESLA_STEERING_LIMITS = {
+    .max_angle = 3600,
+    .angle_deg_to_can = 10,
+    .frequency = 50U,
+  };
+
+  const AngleSteeringParams TESLA_LEGACY_STEERING_PARAMS = {
+    .slip_factor = -0.0005666493436310427,
+    .steer_ratio = 15.,
+    .wheelbase = 2.96,
+  };
+
+  const LongitudinalLimits TESLA_LONG_LIMITS = {
+    .max_accel = 425,
+    .min_accel = 288,
+    .inactive_accel = 375,
+  };
+
+  bool violation = false;
+
+  // DAS_steeringControl: angle is encoded in 0.1 degree units with a 1638.35 offset.
+  if (!tesla_external_panda && (msg->addr == 0x488U)) {
+    const int raw_angle_can = ((msg->data[0] & 0x7FU) << 8) | msg->data[1];
+    const int desired_angle = raw_angle_can - 16384;
+    const int steer_control_type = msg->data[2] >> 6;
+    const bool steer_control_enabled = steer_control_type == 1;
+
+    violation |= steer_angle_cmd_checks_vm(desired_angle, steer_control_enabled,
+                                           TESLA_STEERING_LIMITS, TESLA_LEGACY_STEERING_PARAMS);
+
+    const bool valid_steer_control_type = (steer_control_type == 0) || (steer_control_type == 1);
+    violation |= !valid_steer_control_type;
+    violation |= tesla_legacy_stock_lkas;
+  }
+
+  // DAS_control: HW1 longitudinal control is sent to the powertrain bus (bus 0).
+  if ((tesla_external_panda || tesla_hw1) && (msg->addr == das_control_msg)) {
+    const int aeb_event = msg->data[2] & 0x03U;
+    violation |= aeb_event != 0;
+    violation |= tesla_legacy_stock_aeb;
+
+    const int raw_accel_max = ((msg->data[6] & 0x1FU) << 4) | (msg->data[5] >> 4);
+    const int raw_accel_min = ((msg->data[5] & 0x0FU) << 5) | (msg->data[4] >> 3);
+    if (tesla_legacy_longitudinal) {
+      violation |= (raw_accel_max < TESLA_LONG_LIMITS.inactive_accel) &&
+                   (raw_accel_min < TESLA_LONG_LIMITS.inactive_accel);
+      violation |= longitudinal_accel_checks(raw_accel_max, TESLA_LONG_LIMITS);
+      violation |= longitudinal_accel_checks(raw_accel_min, TESLA_LONG_LIMITS);
+    } else {
+      // Stock ACC may only be cancelled, never spoofed or accelerated.
+      const int acc_state = msg->data[1] >> 4;
+      violation |= acc_state != 13;
+      violation |= (raw_accel_max != TESLA_LONG_LIMITS.inactive_accel) ||
+                   (raw_accel_min != TESLA_LONG_LIMITS.inactive_accel);
+    }
+  }
+
+  return !violation;
+}
+
+static bool tesla_legacy_fwd_hook(int bus_num, int addr) {
+  bool block_msg = false;
+
+  if (bus_num == 2) {
+    if (!tesla_external_panda && !tesla_hw1 && (addr == 0x27dU)) {
+      block_msg = true;
+    }
+    if (!tesla_external_panda && (addr == 0x488U) && !tesla_legacy_stock_lkas) {
+      block_msg = true;
+    }
+    if ((tesla_external_panda || tesla_hw1) && (addr == das_control_msg) && !tesla_legacy_stock_aeb) {
+      block_msg = true;
+    }
+  }
+
+  return block_msg;
+}
+
+static safety_config tesla_legacy_init(uint16_t param) {
+  const int TESLA_FLAG_EXTERNAL_PANDA = 4;
+  const int TESLA_FLAG_HW2 = 16;
+  const int TESLA_FLAG_HW3 = 32;
+
+  tesla_external_panda = GET_FLAG(param, TESLA_FLAG_EXTERNAL_PANDA);
+  tesla_hw1 = GET_FLAG(param, TESLA_LEGACY_FLAG_HW1);
+  tesla_hw2 = GET_FLAG(param, TESLA_FLAG_HW2);
+  tesla_hw3 = GET_FLAG(param, TESLA_FLAG_HW3);
+  tesla_legacy_longitudinal = GET_FLAG(param, 1);
+
+  tesla_legacy_stock_aeb = false;
+  tesla_legacy_stock_lkas = false;
+  tesla_legacy_stock_lkas_prev = false;
+  chassis_bus = 0U;
+  di_torque1_msg = 0x106U;
+  das_control_msg = tesla_external_panda ? 0x2bfU : 0x2b9U;
+
+  static const CanMsg TESLA_TX_LEGACY_MSGS[] = {
+    {0x488, 0, 4, .check_relay = true, .disable_static_blocking = true},
+    {0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true},
+  };
+
+  static const CanMsg TESLA_LEGACY_PT_MSGS[] = {
+    {0x2bf, 0, 8, .check_relay = true, .disable_static_blocking = true},
+  };
+
+  static const CanMsg TESLA_TX_LEGACY_HW1_MSGS[] = {
+    {0x488, 0, 4, .check_relay = true, .disable_static_blocking = true},
+    {0x2b9, 0, 8, .check_relay = true, .disable_static_blocking = true},
+  };
+
+  static RxCheck tesla_legacy_pt_rx_checks[] = {
+    {.msg = {{0x106, 0, 8, 100U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x1f8, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x2bf, 2, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x256, 0, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+  };
+
+  static RxCheck tesla_legacy_hw1_rx_checks[] = {
+    {.msg = {{0x108, 0, 8, 100U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x2b9, 2, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x370, 0, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x155, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x20a, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x368, 0, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x488, 2, 4, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+  };
+
+  static RxCheck tesla_legacy_hw2_rx_checks[] = {
+    {.msg = {{0x370, 0, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x155, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x20a, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x368, 0, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x488, 2, 4, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+  };
+
+  static RxCheck tesla_legacy_hw3_rx_checks[] = {
+    {.msg = {{0x370, 0, 8, 100U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x155, 1, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x20a, 1, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x368, 1, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x488, 2, 4, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+  };
+
+  if (tesla_external_panda && (tesla_hw3 || tesla_hw2)) {
+    return BUILD_SAFETY_CFG(tesla_legacy_pt_rx_checks, TESLA_LEGACY_PT_MSGS);
+  }
+  if (tesla_hw3) {
+    chassis_bus = 1U;
+    return BUILD_SAFETY_CFG(tesla_legacy_hw3_rx_checks, TESLA_TX_LEGACY_MSGS);
+  }
+  if (tesla_hw1) {
+    di_torque1_msg = 0x108U;
+    return BUILD_SAFETY_CFG(tesla_legacy_hw1_rx_checks, TESLA_TX_LEGACY_HW1_MSGS);
+  }
+  return BUILD_SAFETY_CFG(tesla_legacy_hw2_rx_checks, TESLA_TX_LEGACY_MSGS);
+}
+
+const safety_hooks tesla_legacy_hooks = {
+  .init = tesla_legacy_init,
+  .rx = tesla_legacy_rx_hook,
+  .tx = tesla_legacy_tx_hook,
+  .fwd = tesla_legacy_fwd_hook,
+};

opendbc_repo/opendbc/safety/safety.h

@@ -12,6 +12,7 @@
 #include "opendbc/safety/modes/toyota.h"
 #include "opendbc/safety/modes/tesla.h"
 #include "opendbc/safety/modes/tesla_preap.h"
+#include "opendbc/safety/modes/tesla_legacy.h"
 #include "opendbc/safety/modes/gm.h"
 #include "opendbc/safety/modes/ford.h"
 #include "opendbc/safety/modes/hyundai.h"
@@ -502,7 +503,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config);
   for (int i = 0; i < hook_config_count; i++) {
     if (safety_hook_registry[i].id == mode) {
-      current_hooks = safety_hook_registry[i].hooks;
+      current_hooks = ((mode == SAFETY_TESLA) && GET_FLAG(param, TESLA_LEGACY_FLAG_HW1)) ?
+                      &tesla_legacy_hooks : safety_hook_registry[i].hooks;
       current_safety_mode = mode;
       current_safety_param = param;
       set_status = 0;  // set

opendbc_repo/opendbc/safety/tests/test_hyundai_ray_pedal.py

@@ -0,0 +1,49 @@
+import pytest
+
+from opendbc.can import CANPacker
+from opendbc.car import create_gas_interceptor_command
+from opendbc.car.structs import CarParams
+from opendbc.safety.tests.libsafety import libsafety_py
+
+
+@pytest.mark.parametrize("param", [0x9405, 0x9C05, 0x9401, 0x1005, 0])
+def test_ray_pedal_tx_isolation_and_limits(param):
+  safety = libsafety_py.libsafety
+  safety.set_safety_hooks(CarParams.SafetyModel.hyundai, param)
+  safety.init_tests()
+  safety.set_controls_allowed(True)
+  packer = CANPacker("hyundai_kia_ray_pedal")
+
+  def tx(gas):
+    addr, dat, bus = create_gas_interceptor_command(packer, gas, 3)
+    return safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat))
+
+  has_ray_signature = param in (0x9405, 0x9C05)
+  assert tx(0) is has_ray_signature
+  assert tx(0.35) is has_ray_signature
+  assert not tx(0.36)  # above the Ray-only initial command cap
+  assert not tx(1.0)
+
+  if has_ray_signature:
+    safety.set_controls_allowed(False)
+    assert tx(0)
+    assert not tx(0.1)
+    safety.set_controls_allowed(True)
+    safety.set_gas_pressed_prev(True)
+    assert not tx(0.1)
+    safety.set_gas_pressed_prev(False)
+    addr, dat, bus = create_gas_interceptor_command(packer, 0.1, 3)
+    bad_crc = bytearray(dat)
+    bad_crc[-1] ^= 1
+    assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, bytes(bad_crc)))
+
+
+def test_ray_pedal_rx_crc_is_checked_only_for_ray_signature():
+  safety = libsafety_py.libsafety
+  safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0x9405)
+  safety.init_tests()
+  dat = bytes.fromhex("01f403d55de8")
+  assert safety.safety_rx_hook(libsafety_py.make_CANPacket(0x201, 0, dat))
+  bad_crc = bytearray(dat)
+  bad_crc[-1] ^= 1
+  assert not safety.safety_rx_hook(libsafety_py.make_CANPacket(0x201, 0, bytes(bad_crc)))

opendbc_repo/opendbc/safety/tests/test_tesla_legacy.py

@@ -0,0 +1,73 @@
+import pytest
+
+from opendbc.can import CANPacker
+from opendbc.car import Bus
+from opendbc.car.tesla.teslacan_legacy import TeslaCANRaven
+from opendbc.car.tesla.values import CANBUS, CAR, DBC, TeslaSafetyFlags
+from opendbc.safety.tests.libsafety import libsafety_py
+
+
+@pytest.fixture
+def legacy_safety():
+  safety = libsafety_py.libsafety
+  packer = CANPacker(DBC[CAR.TESLA_MODEL_S_HW1][Bus.pt])
+  return safety, TeslaCANRaven({CANBUS.party: packer})
+
+
+def tx(safety, msg):
+  addr, data, bus = msg
+  return safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, data))
+
+
+def test_hw1_steering_requires_controls_allowed(legacy_safety):
+  safety, can = legacy_safety
+  safety.set_safety_hooks(10, TeslaSafetyFlags.FLAG_HW1.value)
+  safety.init_tests()
+  safety.set_angle_meas(0, 0)
+  safety.set_controls_allowed(False)
+  assert tx(safety, can.create_steering_control(0, 0, False))
+  assert not tx(safety, can.create_steering_control(0, 0, True))
+  safety.set_controls_allowed(True)
+  assert tx(safety, can.create_steering_control(0, 0, True))
+
+
+@pytest.mark.parametrize("alpha_long", [False, True])
+def test_hw1_accel_only_allowed_with_alpha_long_and_engagement(legacy_safety, alpha_long):
+  safety, can = legacy_safety
+  param = TeslaSafetyFlags.FLAG_HW1.value | (TeslaSafetyFlags.LONG_CONTROL.value if alpha_long else 0)
+  safety.set_safety_hooks(10, param)
+  safety.init_tests()
+  safety.set_controls_allowed(True)
+  assert tx(safety, can.create_longitudinal_command(13, 0, 0, 10, False, False))
+  assert tx(safety, can.create_longitudinal_command(4, 1, 0, 10, True, False)) == alpha_long
+  safety.set_controls_allowed(False)
+  assert not tx(safety, can.create_longitudinal_command(4, 1, 0, 10, True, False))
+
+
+def test_hw1_stock_ap_steer_and_acc_are_blocked_from_forwarding(legacy_safety):
+  safety, _ = legacy_safety
+  safety.set_safety_hooks(10, TeslaSafetyFlags.FLAG_HW1.value)
+  safety.init_tests()
+  assert safety.safety_fwd_hook(2, 0x488) == -1
+  assert safety.safety_fwd_hook(2, 0x2b9) == -1
+  assert safety.safety_fwd_hook(2, 0x370) == 0
+
+
+def test_hw1_flag_dispatch_does_not_change_modern_or_preap_hooks(legacy_safety):
+  safety, can = legacy_safety
+  steer = can.create_steering_control(0, 0, False)
+  safety.set_safety_hooks(10, TeslaSafetyFlags.FLAG_HW1.value)
+  safety.init_tests()
+  assert tx(safety, steer)
+
+  # APS monitor exists only in the modern Tesla TX whitelist; HW1 must not
+  # accidentally inherit it from the unflagged hook.
+  monitor = libsafety_py.make_CANPacket(0x27d, 0, bytes(3))
+  assert not safety.safety_tx_hook(monitor)
+  safety.set_safety_hooks(10, 0)
+  safety.init_tests()
+  assert safety.safety_tx_hook(monitor)
+
+  safety.set_safety_hooks(35, 0)
+  safety.init_tests()
+  assert safety.safety_fwd_hook(2, 0x370) == -1

opendbc_repo/pyproject.toml

@@ -130,4 +130,5 @@ flake8-implicit-str-concat.allow-multiline=false
 include-package-data = true
 
 [tool.setuptools.package-data]
+"opendbc.dbc" = ["hyundai_kia_ray_pedal.dbc"]
 "opendbc.safety" = ["*.h", "board/*.h", "board/drivers/*.h", "modes/*.h"]

panda/board/obj/gitversion.h

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

panda/board/obj/version

@@ -1 +1 @@
-DEV-c51b9687-DEBUG
+DEV-3ba36ed4-DEBUG

selfdrive/controls/lib/desire_helper.py

@@ -14,6 +14,13 @@ LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
 NAV_TURN_DISTANCE_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0]
 NAV_TURN_DISTANCE_BREAKPOINTS = [20.0, 25.0, 30.0]
+# A driver normally signals an intersection before slowing below the lane-change
+# speed threshold. Use the route to classify that early signal so it does not
+# start a lane change while approaching the matching turn.
+NAV_TURN_SIGNAL_LEAD_TIME = 12.0
+NAV_TURN_SIGNAL_BASE_DISTANCE = 15.0
+NAV_TURN_SIGNAL_MIN_DISTANCE = 30.0
+NAV_TURN_SIGNAL_MAX_DISTANCE = 250.0
 NAV_KEEP_DISTANCE_SPEED_BREAKPOINTS = [0.0, 15.0, 30.0]
 NAV_KEEP_DISTANCE_BREAKPOINTS = [25.0, 90.0, 160.0]
 NAV_KEEP_AMBIGUOUS_SPLIT_DISTANCE_SCALE = 0.6
@@ -117,6 +124,34 @@ class DesireHelper:
 
     return distance <= float(np.interp(carstate.vEgo, NAV_TURN_DISTANCE_SPEED_BREAKPOINTS, NAV_TURN_DISTANCE_BREAKPOINTS))
 
+  @staticmethod
+  def _nav_turn_signal_matches(carstate, nav_instruction_state):
+    if not bool(nav_instruction_state.get("valid", False)):
+      return False
+    if str(nav_instruction_state.get("maneuverType", "")).strip().lower() != "turn":
+      return False
+
+    modifier = str(nav_instruction_state.get("maneuverModifier", "")).strip()
+    matching_signal = (
+      modifier in ("left", "sharpLeft") and carstate.leftBlinker and not carstate.rightBlinker
+    ) or (
+      modifier in ("right", "sharpRight") and carstate.rightBlinker and not carstate.leftBlinker
+    )
+    if not matching_signal:
+      return False
+
+    try:
+      maneuver_distance = float(nav_instruction_state.get("maneuverDistance", 0.0))
+    except (TypeError, ValueError):
+      return False
+
+    signal_distance = float(np.clip(
+      NAV_TURN_SIGNAL_BASE_DISTANCE + max(float(carstate.vEgo), 0.0) * NAV_TURN_SIGNAL_LEAD_TIME,
+      NAV_TURN_SIGNAL_MIN_DISTANCE,
+      NAV_TURN_SIGNAL_MAX_DISTANCE,
+    ))
+    return 0.0 <= maneuver_distance <= signal_distance
+
   @staticmethod
   def _nudgeless_enabled(starpilot_toggles, controls_enabled):
     nudgeless = bool(getattr(starpilot_toggles, "nudgeless", False))
@@ -245,6 +280,10 @@ class DesireHelper:
     one_blinker = carstate.leftBlinker != carstate.rightBlinker
     below_lane_change_speed = v_ego < starpilot_toggles.minimum_lane_change_speed
 
+    self._update_nav_params()
+    self.nav_desires_allowed = bool(getattr(starpilot_toggles, "nav_desires_allowed", self.nav_desires_allowed))
+    nav_turn_signal = self.nav_desires_allowed and self._nav_turn_signal_matches(carstate, self._nav_instruction_state)
+
     stop_imminent = (bool(getattr(starpilotPlan, "redLight", False))
                      or bool(getattr(starpilotPlan, "forcingStop", False))
                      or bool(getattr(starpilotPlan, "stopSignConfirmed", False)))
@@ -264,8 +303,13 @@ class DesireHelper:
       self.lane_change_state = LaneChangeState.off
       self.lane_change_direction = LaneChangeDirection.none
     else:
+      if nav_turn_signal and self.lane_change_state == LaneChangeState.preLaneChange:
+        self.lane_change_state = LaneChangeState.off
+        self.lane_change_direction = LaneChangeDirection.none
+
       # LaneChangeState.off
-      if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
+      if (self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker
+          and not below_lane_change_speed and not nav_turn_signal):
         self.lane_change_state = LaneChangeState.preLaneChange
         self.lane_change_ll_prob = 1.0
         # Initialize lane change direction to prevent UI alert flicker

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -268,8 +268,8 @@ GENESIS_GV70_OUTPUT_SMOOTHING_SPEED_WIDTH = 6.0 * CV.MPH_TO_MS
 GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT = 0.48
 GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH = 0.16
 GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_RC = 0.42
-GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC = 0.14
-GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC = 0.12
+GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC = 0.20
+GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC = 0.16
 GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04
 GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08
 GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.55
@@ -360,6 +360,13 @@ GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04
 GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08
 GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.45
 GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.28
+GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED = 45.0 * CV.MPH_TO_MS
+GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED_WIDTH = 5.0 * CV.MPH_TO_MS
+GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT = 0.35
+GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT_WIDTH = 0.15
+GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK = 0.25
+GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK_WIDTH = 0.15
+GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_RC = 0.55
 GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45
 GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0
 GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0
@@ -654,7 +661,7 @@ KIA_CARNIVAL_UNWIND_FF_OVERSHOOT = 0.08
 KIA_CARNIVAL_UNWIND_FF_OVERSHOOT_WIDTH = 0.06
 KIA_CARNIVAL_UNWIND_FF_JERK = 0.45
 KIA_CARNIVAL_UNWIND_FF_JERK_WIDTH = 0.20
-KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_MAX = 0.28
+KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_MAX = 0.35
 KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED = 8.0
 KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_WIDTH = 2.0
 KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_CUTOFF = 16.0
@@ -1321,7 +1328,7 @@ KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT = 0.20
 KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.05
 KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED = 18.0
 KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.5
-KONA_EV_2022_CENTER_OUTPUT_TAPER_MAX = 0.045
+KONA_EV_2022_CENTER_OUTPUT_TAPER_MAX = 0.08
 KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT = 0.20
 KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.05
 KONA_EV_2022_CENTER_OUTPUT_TAPER_SPEED = 18.0
@@ -3485,6 +3492,24 @@ def get_genesis_g70_stabilized_output(output_torque: float, prev_output_torque:
   if changing_direction:
     response_time = max(response_time, GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC)
 
+  output_reversal = (prev_output_torque * output_torque < -0.0025 and
+                     abs(desired_lateral_accel) >= GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT)
+  if output_reversal:
+    reversal_speed_weight = _sigmoid(
+      (max(v_ego, 0.0) - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED) /
+      GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED_WIDTH
+    )
+    reversal_lat_weight = _sigmoid(
+      (abs(desired_lateral_accel) - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT) /
+      GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT_WIDTH
+    )
+    reversal_jerk_weight = _sigmoid(
+      (abs(desired_lateral_jerk) - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK) /
+      GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK_WIDTH
+    )
+    reversal_weight = reversal_speed_weight * reversal_lat_weight * reversal_jerk_weight
+    response_time += reversal_weight * max(GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_RC - response_time, 0.0)
+
   output_alpha = dt / (max(response_time, 0.0) + dt)
   smoothed_output = prev_output_torque + output_alpha * (output_torque - prev_output_torque)
   return float(output_torque + speed_weight * (smoothed_output - output_torque))

selfdrive/controls/tests/test_latcontrol.py

@@ -767,6 +767,7 @@ class TestLatControl:
     assert steady_turn == pytest.approx(1.0)
     assert clean_unwind == pytest.approx(1.0)
     assert 0.70 < overshooting_unwind < 1.0
+    assert overshooting_unwind < 0.80
     assert high_speed_overshoot > overshooting_unwind
 
   def test_genesis_g90_ff_scale_curve(self):
@@ -1399,9 +1400,9 @@ class TestLatControl:
     assert low_speed_threshold == pytest.approx(get_standard_friction_threshold(8.0), abs=0.001)
     assert highway_threshold > highway_base
     assert highway_curve_threshold == pytest.approx(highway_base, abs=0.001)
-    assert get_kona_ev_2022_center_output_scale(0.0, 27.0) < 0.96
+    assert get_kona_ev_2022_center_output_scale(0.0, 27.0) < 0.94
     assert get_kona_ev_2022_center_output_scale(0.6, 27.0) == pytest.approx(1.0, abs=0.001)
-    assert get_kona_ev_2022_center_output_scale(0.0, 8.0) == pytest.approx(1.0, abs=0.001)
+    assert get_kona_ev_2022_center_output_scale(0.0, 8.0) == pytest.approx(1.0, abs=0.002)
 
   def test_kona_ev_2022_center_output_taper_update_path(self, monkeypatch):
     monkeypatch.setattr(latcontrol_torque, "get_kona_ev_2022_center_output_scale", lambda *_args: 1.0)
@@ -1830,11 +1831,13 @@ class TestLatControl:
     high_speed_wind = get_genesis_g70_stabilized_output(0.1, 0.3, 0.8, 0.5, 30.0, DT_CTRL)
     high_speed_unwind = get_genesis_g70_stabilized_output(0.1, 0.3, 0.8, -0.5, 30.0, DT_CTRL)
     high_speed_direction_change = get_genesis_g70_stabilized_output(-0.3, 0.3, -0.8, -0.5, 30.0, DT_CTRL)
+    low_speed_direction_change = get_genesis_g70_stabilized_output(-0.3, 0.3, -0.8, -0.5, 10.0, DT_CTRL)
 
     assert low_speed == pytest.approx(-0.2, abs=0.005)
     assert abs(high_speed_center - 0.2) < abs(low_speed - 0.2)
     assert high_speed_unwind > high_speed_wind > 0.1
     assert 0.2 < high_speed_direction_change < 0.3
+    assert high_speed_direction_change > low_speed_direction_change
 
   def test_genesis_g70_output_stabilizer_update_path(self, monkeypatch):
     calls = []

selfdrive/controls/tests/test_navigation_desires.py

@@ -116,6 +116,76 @@ def test_nav_desires_turn_right_waits_until_turn_is_close():
   assert helper.desire == log.Desire.none
 
 
+def test_matching_routed_turn_does_not_start_lane_change_above_threshold():
+  helper = DesireHelper()
+  helper._update_nav_params = lambda: None
+  helper._nav_instruction_state = {
+    "valid": True,
+    "maneuverType": "turn",
+    "maneuverModifier": "right",
+    "maneuverDistance": 111.0,
+  }
+
+  helper.update(
+    make_car_state(vEgo=16.0, rightBlinker=True),
+    True,
+    0.0,
+    make_plan(),
+    make_toggles(minimum_lane_change_speed=11.1),
+  )
+
+  assert helper.lane_change_state == LaneChangeState.off
+  assert helper.lane_change_direction == LaneChangeDirection.none
+  assert helper.desire == log.Desire.none
+
+
+def test_distant_routed_turn_does_not_block_lane_change():
+  helper = DesireHelper()
+  helper._update_nav_params = lambda: None
+  helper._nav_instruction_state = {
+    "valid": True,
+    "maneuverType": "turn",
+    "maneuverModifier": "right",
+    "maneuverDistance": 794.0,
+  }
+
+  helper.update(
+    make_car_state(vEgo=16.0, rightBlinker=True),
+    True,
+    0.0,
+    make_plan(),
+    make_toggles(minimum_lane_change_speed=11.1),
+  )
+
+  assert helper.lane_change_state == LaneChangeState.preLaneChange
+  assert helper.lane_change_direction == LaneChangeDirection.right
+
+
+def test_matching_routed_turn_cancels_pending_lane_change_before_it_starts():
+  helper = DesireHelper()
+  helper._update_nav_params = lambda: None
+  helper._nav_instruction_state = {
+    "valid": True,
+    "maneuverType": "turn",
+    "maneuverModifier": "left",
+    "maneuverDistance": 125.0,
+  }
+  helper.lane_change_state = LaneChangeState.preLaneChange
+  helper.lane_change_direction = LaneChangeDirection.left
+  helper.prev_one_blinker = True
+
+  helper.update(
+    make_car_state(vEgo=11.0, leftBlinker=True),
+    True,
+    0.0,
+    make_plan(),
+    make_toggles(minimum_lane_change_speed=10.0),
+  )
+
+  assert helper.lane_change_state == LaneChangeState.off
+  assert helper.lane_change_direction == LaneChangeDirection.none
+
+
 def test_nav_desires_off_ramp_lane_guidance_becomes_keep_right():
   helper = DesireHelper()
   helper.nav_desires_allowed = True

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -1284,6 +1284,33 @@ def test_nav_turn_speed_control_slows_for_imminent_turn():
   assert result > 0.0
 
 
+def test_nav_turn_speed_control_begins_before_reported_intersection_approach():
+  _, vcruise = make_vcruise(nav_state={
+    "valid": True,
+    "maneuverType": "turn",
+    "maneuverModifier": "right",
+    "maneuverDistance": 111.0,
+    "nextManeuverType": "",
+    "nextManeuverModifier": "",
+    "nextManeuverDistance": 0.0,
+  })
+
+  toggles = make_toggles()
+  toggles.nav_longitudinal_allowed = True
+  result = vcruise.update(
+    controls_enabled=True,
+    now=0.0,
+    time_validated=True,
+    v_cruise=16.1,
+    v_ego=16.0,
+    sm=make_sm(standstill=False),
+    starpilot_toggles=toggles,
+  )
+
+  assert result < 16.1
+  assert result == pytest.approx(vcruise.nav_turn_target)
+
+
 def test_nav_turn_speed_control_ignores_distant_turn():
   _, vcruise = make_vcruise(nav_state={
     "valid": True,

starpilot/car/ford/lateral.py

@@ -47,17 +47,22 @@ MACH_E_DIRECTION_CHANGE_MIN_SPEED = 9.0
 MACH_E_DIRECTION_CHANGE_LOOKAHEAD_RAMP_SPEED = 10.0
 MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FULL_SPEED = 12.0
 MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FADE_SPEED = 15.0
-MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA = 1.60
+MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA = 2.40
 MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE = 0.0005
 MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE = 0.002
 MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE = 0.0008
 MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE = 0.0015
+MACH_E_DIRECTION_CHANGE_EARLY_MIN_LAG_CURVATURE = -0.001
+MACH_E_DIRECTION_CHANGE_EARLY_FULL_LAG_CURVATURE = 0.0008
+MACH_E_DIRECTION_CHANGE_EARLY_MIN_CURVATURE = 0.002
+MACH_E_DIRECTION_CHANGE_EARLY_FULL_CURVATURE = 0.004
 MACH_E_LOW_SPEED_DIRECTION_CHANGE_START_SPEED = 1.8
 MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_SPEED = 2.0
 MACH_E_LOW_SPEED_DIRECTION_CHANGE_HOLD_SPEED = 2.8
 MACH_E_LOW_SPEED_DIRECTION_CHANGE_FADE_SPEED = 3.5
 MACH_E_LOW_SPEED_DIRECTION_CHANGE_MIN_CURVATURE = 0.0004
 MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_CURVATURE = 0.0006
+MACH_E_LOW_SPEED_DIRECTION_CHANGE_MAX_CURVATURE = 0.0015
 FORD_CURVATURE_LOOKAHEAD = {
   CAR.FORD_EXPLORER_MK6: 0.20,
 }
@@ -246,13 +251,27 @@ class FordLateralController:
        MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA, MACH_E_TURN_IN_LOOKAHEAD_EXTRA],
     ))
 
-  def _direction_change_preview_weight(self, desired: float, preview: float, current: float) -> float:
+  def _direction_change_preview_weight(self, desired: float, preview: float, current: float,
+                                       allow_rising_desired: bool = False, early_handoff_weight: float = 0.0) -> float:
     if self.CP.carFingerprint not in FORD_CONSERVATIVE_PREVIEW_CARS:
       return 0.0
-    if desired * preview >= 0.0 or desired * self.desired_curvature_last <= 0.0:
+    if desired * preview >= 0.0 or desired * self.desired_curvature_last <= 0.0 or desired * current <= 0.0:
       return 0.0
-    if (abs(desired) >= abs(self.desired_curvature_last) or desired * current <= 0.0 or
-        abs(current) <= abs(desired)):
+    early_handoff_weight = float(np.clip(early_handoff_weight, 0.0, 1.0))
+    lag = abs(current) - abs(desired)
+    lag_min = float(np.interp(
+      early_handoff_weight, [0.0, 1.0],
+      [MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_MIN_LAG_CURVATURE],
+    ))
+    lag_full = float(np.interp(
+      early_handoff_weight, [0.0, 1.0],
+      [MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_FULL_LAG_CURVATURE],
+    ))
+    desired_rising = abs(desired) >= abs(self.desired_curvature_last)
+    rising_handoff = (allow_rising_desired and abs(desired) > abs(self.desired_curvature_last) and
+                      abs(desired) <= MACH_E_LOW_SPEED_DIRECTION_CHANGE_MAX_CURVATURE)
+    early_rising_handoff = early_handoff_weight > 0.0 and lag > lag_min
+    if desired_rising and not rising_handoff and not early_rising_handoff:
       return 0.0
 
     preview_weight = float(np.interp(
@@ -261,8 +280,8 @@ class FordLateralController:
       [0.0, 1.0],
     ))
     lag_weight = float(np.interp(
-      abs(current) - abs(desired),
-      [MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE],
+      lag,
+      [lag_min, lag_full],
       [0.0, 1.0],
     ))
     return preview_weight * lag_weight
@@ -351,13 +370,26 @@ class FordLateralController:
       direction_change_predicted = turn_in_predicted
       direction_change_weight = 0.0
       direction_change_speed_weight = float(v_ego > MACH_E_DIRECTION_CHANGE_MIN_SPEED)
+      low_speed_direction_change = direction_change_speed_weight == 0.0
       if direction_change_speed_weight == 0.0:
         direction_change_speed_weight = self._low_speed_direction_change_weight(v_ego, desired)
       if direction_change_speed_weight > 0.0 and not CS.out.steeringPressed and not self._lane_change()[0]:
         direction_change_lookahead_extra = self._direction_change_lookahead_extra(v_ego)
+        early_handoff_weight = float(np.interp(
+          direction_change_lookahead_extra,
+          [MACH_E_TURN_IN_LOOKAHEAD_EXTRA, MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA],
+          [0.0, 1.0],
+        ))
+        early_handoff_weight *= float(np.interp(
+          abs(desired),
+          [MACH_E_DIRECTION_CHANGE_EARLY_MIN_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_FULL_CURVATURE],
+          [0.0, 1.0],
+        ))
         if direction_change_lookahead_extra > MACH_E_TURN_IN_LOOKAHEAD_EXTRA:
           direction_change_predicted = self._predicted_curvature(v_ego, lookahead + direction_change_lookahead_extra)
-        direction_change_weight = self._direction_change_preview_weight(desired, direction_change_predicted, current)
+        direction_change_weight = self._direction_change_preview_weight(
+          desired, direction_change_predicted, current, allow_rising_desired=low_speed_direction_change,
+          early_handoff_weight=early_handoff_weight)
         direction_change_weight *= direction_change_speed_weight
       if direction_change_weight > 0.0:
         predicted = float(np.interp(direction_change_weight, [0.0, 1.0], [predicted, direction_change_predicted]))

starpilot/car/ford/tests/test_lateral.py

@@ -188,10 +188,10 @@ def test_mach_e_turn_in_lookahead_extra_fades_by_speed(controller, speed, expect
 @pytest.mark.parametrize("speed,expected", (
   (8.0, 0.80),
   (9.0, 0.80),
-  (9.5, 1.20),
-  (10.0, 1.60),
-  (12.0, 1.60),
-  (13.5, 1.20),
+  (9.5, 1.60),
+  (10.0, 2.40),
+  (12.0, 2.40),
+  (13.5, 1.60),
   (15.0, 0.80),
   (16.0, 0.80),
 ))
@@ -225,6 +225,52 @@ def test_mach_e_direction_change_preview_leads_a_lagging_unwind(controller, sign
   assert weight == pytest.approx(1.0)
 
 
+def test_mach_e_low_speed_direction_change_preview_can_lead_a_rising_near_path(controller):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.desired_curvature_last = 0.0006
+
+  assert controller._direction_change_preview_weight(
+    desired=0.0008, preview=-0.002, current=0.003, allow_rising_desired=True) == pytest.approx(1.0)
+  assert controller._direction_change_preview_weight(
+    desired=0.0008, preview=-0.002, current=0.003) == 0.0
+
+
+def test_mach_e_low_speed_direction_change_preview_rejects_large_rising_near_path(controller):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.desired_curvature_last = 0.0014
+
+  assert controller._direction_change_preview_weight(
+    desired=0.0016, preview=-0.002, current=0.003, allow_rising_desired=True) == 0.0
+
+
+def test_mach_e_extended_direction_preview_begins_before_measured_curvature_catches_desired(controller):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.desired_curvature_last = 0.0148
+
+  assert controller._direction_change_preview_weight(
+    desired=0.0147, preview=-0.002, current=0.0140, early_handoff_weight=1.0) == pytest.approx(1.0 / 6.0)
+  assert controller._direction_change_preview_weight(
+    desired=0.0147, preview=-0.002, current=0.0140) == 0.0
+
+
+def test_mach_e_extended_direction_preview_tolerates_small_desired_jitter(controller):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.desired_curvature_last = 0.0146
+
+  assert controller._direction_change_preview_weight(
+    desired=0.0147, preview=-0.002, current=0.0141, early_handoff_weight=1.0) == pytest.approx(2.0 / 9.0)
+  assert controller._direction_change_preview_weight(
+    desired=0.0147, preview=-0.002, current=0.0141) == 0.0
+
+
+def test_mach_e_extended_direction_preview_preserves_turn_in_when_vehicle_lags(controller):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.desired_curvature_last = 0.0146
+
+  assert controller._direction_change_preview_weight(
+    desired=0.0147, preview=-0.002, current=0.0130, early_handoff_weight=1.0) == 0.0
+
+
 @pytest.mark.parametrize("desired,preview,current,last", (
   (0.0015, 0.002, 0.003, 0.002),     # no predicted direction change
   (0.002, -0.002, 0.003, 0.0015),    # desired curvature is still rising
@@ -296,6 +342,51 @@ def test_mach_e_direction_change_preview_leads_low_speed_handoff(controller, mon
                            pytest.approx(0.003), True)]
 
 
+def test_mach_e_direction_change_preview_leads_rising_low_speed_handoff(controller, monkeypatch):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.sm["liveDelay"].lateralDelay = 0.4
+  controller.desired_curvature_last = 0.0006
+  blend_inputs = []
+  monkeypatch.setattr(
+    controller, "_predicted_curvature",
+    lambda _v_ego, lookahead: 0.0008 if lookahead < 1.0 else -0.002,
+  )
+  monkeypatch.setattr(
+    controller, "_blend_and_scale",
+    lambda desired, predicted, v_ego, current, allow_opposite_preview=False:
+      blend_inputs.append((desired, predicted, v_ego, current, allow_opposite_preview)) or (0.0, 1),
+  )
+
+  controller.update(
+    SimpleNamespace(latActive=True), car_state(speed=2.5, curvature=0.003),
+    SimpleNamespace(curvature=0.0008),
+  )
+
+  assert blend_inputs == [(pytest.approx(0.0008), pytest.approx(-0.002), pytest.approx(2.5),
+                           pytest.approx(0.003), True)]
+
+
+def test_mach_e_direction_change_preview_does_not_lead_rising_high_speed_path(controller, monkeypatch):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.sm["liveDelay"].lateralDelay = 0.4
+  controller.desired_curvature_last = 0.0006
+  blend_inputs = []
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda _v_ego, _lookahead: -0.002)
+  monkeypatch.setattr(
+    controller, "_blend_and_scale",
+    lambda desired, predicted, v_ego, current, allow_opposite_preview=False:
+      blend_inputs.append((desired, predicted, v_ego, current, allow_opposite_preview)) or (0.0, 1),
+  )
+
+  controller.update(
+    SimpleNamespace(latActive=True), car_state(speed=15.0, curvature=0.003),
+    SimpleNamespace(curvature=0.0008),
+  )
+
+  assert blend_inputs == [(pytest.approx(0.0008), pytest.approx(-0.002), pytest.approx(15.0),
+                           pytest.approx(0.003), False)]
+
+
 def test_mach_e_direction_change_preview_uses_extended_horizon_at_medium_speed(controller, monkeypatch):
   controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
   controller.sm["liveDelay"].lateralDelay = 0.4
@@ -305,7 +396,7 @@ def test_mach_e_direction_change_preview_uses_extended_horizon_at_medium_speed(c
 
   def predicted_curvature(_v_ego, lookahead):
     lookaheads.append(lookahead)
-    return {0.4: 0.002, 1.2: 0.001, 2.0: -0.002}[round(lookahead, 1)]
+    return {0.4: 0.002, 1.2: 0.001, 2.8: -0.002}[round(lookahead, 1)]
 
   monkeypatch.setattr(controller, "_predicted_curvature", predicted_curvature)
   monkeypatch.setattr(
@@ -319,11 +410,63 @@ def test_mach_e_direction_change_preview_uses_extended_horizon_at_medium_speed(c
     SimpleNamespace(curvature=0.0015),
   )
 
-  assert lookaheads == [pytest.approx(0.4), pytest.approx(1.2), pytest.approx(2.0)]
+  assert lookaheads == [pytest.approx(0.4), pytest.approx(1.2), pytest.approx(2.8)]
   assert blend_inputs == [(pytest.approx(0.0015), pytest.approx(-0.002), pytest.approx(10.5),
                            pytest.approx(0.003), True)]
 
 
+def test_mach_e_extended_direction_preview_advances_large_curve_exit(controller, monkeypatch):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.sm["liveDelay"].lateralDelay = 0.4
+  controller.desired_curvature_last = 0.0146
+  blend_inputs = []
+
+  def predicted_curvature(_v_ego, lookahead):
+    return {0.4: 0.0144, 1.2: 0.011, 2.8: -0.002}[round(lookahead, 1)]
+
+  monkeypatch.setattr(controller, "_predicted_curvature", predicted_curvature)
+  monkeypatch.setattr(
+    controller, "_blend_and_scale",
+    lambda desired, predicted, v_ego, current, allow_opposite_preview=False:
+      blend_inputs.append((desired, predicted, v_ego, current, allow_opposite_preview)) or (0.0, 1),
+  )
+
+  controller.update(
+    SimpleNamespace(latActive=True), car_state(speed=12.0, curvature=0.0141),
+    SimpleNamespace(curvature=0.0147),
+  )
+
+  assert len(blend_inputs) == 1
+  assert blend_inputs[0][0] == pytest.approx(0.0147)
+  assert blend_inputs[0][1] < 0.0144
+  assert blend_inputs[0][4]
+
+
+def test_mach_e_extended_direction_preview_preserves_small_medium_speed_path(controller, monkeypatch):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.sm["liveDelay"].lateralDelay = 0.4
+  controller.desired_curvature_last = 0.0007
+  blend_inputs = []
+
+  def predicted_curvature(_v_ego, lookahead):
+    return 0.0008 if lookahead < 2.0 else -0.002
+
+  monkeypatch.setattr(controller, "_predicted_curvature", predicted_curvature)
+  monkeypatch.setattr(
+    controller, "_blend_and_scale",
+    lambda desired, predicted, v_ego, current, allow_opposite_preview=False:
+      blend_inputs.append((desired, predicted, v_ego, current, allow_opposite_preview)) or (0.0, 1),
+  )
+
+  controller.update(
+    SimpleNamespace(latActive=True), car_state(speed=12.0, curvature=0.0014),
+    SimpleNamespace(curvature=0.0008),
+  )
+
+  assert blend_inputs == [(pytest.approx(0.0008), pytest.approx(0.0008), pytest.approx(12.0),
+                           pytest.approx(0.0014), False)]
+
+
 def test_mach_e_extended_direction_horizon_does_not_replace_turn_in_preview(controller, monkeypatch):
   controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
   controller.sm["liveDelay"].lateralDelay = 0.4
@@ -331,7 +474,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.0: -0.002}[round(lookahead, 1)]
+    return {0.4: 0.006, 1.2: 0.010, 1.6: 0.004, 2.8: -0.002}[round(lookahead, 1)]
 
   monkeypatch.setattr(controller, "_predicted_curvature", predicted_curvature)
   monkeypatch.setattr(

starpilot/controls/lib/starpilot_vcruise.py

@@ -35,7 +35,10 @@ SLC_LEAD_DROP_RELAXATION_MAX_POST_DROP_CLOSING_SPEED = 0.35
 SLC_LEAD_DROP_RELAXATION_MAX_LEAD_BRAKE = 0.25
 SLC_LEAD_DROP_RELAXATION_OVERSPEED_BP = [0.0, 5.0 * CV.MPH_TO_MS, 10.0 * CV.MPH_TO_MS, 15.0 * CV.MPH_TO_MS]
 SLC_LEAD_DROP_RELAXATION_DECEL_V = [0.7, 0.9, 1.15, 1.35]
-NAV_TURN_COMFORT_DECEL = 1.25
+# This is an approach envelope, not a request for harder braking. A gentler
+# deceleration value lowers the target farther from the turn and gives the MPC
+# more time to settle before the intersection.
+NAV_TURN_COMFORT_DECEL = 0.85
 NAV_TURN_DISTANCE_BUFFER = 8.0
 NAV_TURN_MIN_TARGET_DELTA = 0.25
 NAV_TURN_TARGET_SPEEDS = {