Joystick Mode Refresh

OP Long: Left stick horizontal steer
	            Right stick vertical gas/brake

AOL:  Left stick horizontal steer

Controller profiles added:
Google Stadia Controller added
Xbox Controller has its own profile
Dual Sense has its own profile

joystick_probe.py tool allows people to discover their own controllers.

cereal/custom.capnp

@@ -220,6 +220,10 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
   disableThrottle @35 :Bool;
   trackingLead @36 :Bool;
   stopSignConfirmed @37 :Bool;
+  # Curve Speed Controller diagnostics, for tuning and rollout validation
+  cscOverridden @38 :Bool;          # driver cancelled this curve with RES+
+  cscLearnedLatAccel @39 :Float32;  # learned comfort at the current curvature, before margin
+  cscBindingDistance @40 :Float32;  # distance to the horizon point setting the target, m
 }
 
 struct StarPilotRadarState @0xb86e6369214c01c8 {

cereal/log.capnp

@@ -507,7 +507,7 @@ struct DeviceState @0xa4d8b5af2aa492eb {
   pmicTempC @39 :List(Float32);
   intakeTempC @46 :Float32;
   exhaustTempC @47 :Float32;
-  caseTempC @48 :Float32;
+  gnssTempC @48 :Float32;
   bottomSocTempC @50 :Float32;
   maxTempC @44 :Float32;  # max of other temps, used to control fan
   thermalZones @38 :List(ThermalZone);
@@ -522,10 +522,10 @@ struct DeviceState @0xa4d8b5af2aa492eb {
   }
 
   enum ThermalStatus {
-    green @0;
-    yellow @1;
-    red @2;
-    danger @3;
+    ok @0;
+    warmDEPRECATED @1;
+    overheated @2;
+    critical @3;
   }
 
   enum NetworkType {

common/params_keys.h

@@ -241,6 +241,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"CurvatureData", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}},
     {"CurveSpeedController", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
     {"CurveSpeedControllerNoLead", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
+    {"CurveSpeedApproachDecel", {PERSISTENT, FLOAT, "0.6", "0.6", 2, SETTINGS_SIMPLE}},
+    {"CurveSpeedMargin", {PERSISTENT, INT, "85", "85", 2, SETTINGS_SIMPLE}},
     {"CustomAlerts", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
     {"CustomAccelProfile", {PERSISTENT, BOOL, "0", "0", 3}},
     {"CustomAccelProfileInitialized", {PERSISTENT, BOOL, "0", "0", 3}},

opendbc_repo/opendbc/car/hyundai/carstate.py

@@ -28,6 +28,11 @@ IONIQ_6_BLINDSPOT_LEFT_MASK = 0x10
 CANFD_CAMERA_LEAD_MIN_DISTANCE = 0.1
 ALT_BUS_LDA_BUTTON_BURST_DEBOUNCE_NS = int(1.3e9)
 
+CLASSIC_MEDIA_BUTTON_CARS = frozenset({
+  CAR.HYUNDAI_ELANTRA_2024,
+  CAR.HYUNDAI_ELANTRA_HEV_2024,
+})
+
 
 def get_non_scc_cruise_signals(CP) -> tuple[str, str, str, str, str, str]:
   if CP.flags & HyundaiFlags.EV:
@@ -437,6 +442,9 @@ class CarState(CarStateBase):
     ret.lowSpeedAlert = self.low_speed_alert
 
     fp_ret = custom.StarPilotCarState.new_message()
+    if self.CP.carFingerprint in CLASSIC_MEDIA_BUTTON_CARS:
+      fp_ret.modePressed = bool(cp.vl["GW_SWRC_PE"]["C_ModeSW"])
+      fp_ret.customPressed = bool(cp.vl["GW_SWRC_PE"]["C_MTSSW"])
 
     return ret, fp_ret
 
@@ -686,6 +694,9 @@ class CarState(CarStateBase):
       ("BCM_PO_11", 0),
       ("CLU13", 0),
     ]
+    if CP.carFingerprint in CLASSIC_MEDIA_BUTTON_CARS:
+      # Steering-wheel media switches are event-driven on the refresh Elantra.
+      msgs.append(("GW_SWRC_PE", 0))
     if CP.flags & HyundaiFlags.NON_SCC and not (CP.flags & HyundaiFlags.NON_SCC_NO_FCA):
       msgs.append(("FCA11", 0))  # Non-SCC trims can stop publishing FCA11; don't let it poison canValid
 

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

@@ -654,6 +654,22 @@ class TestHyundaiFingerprint:
     assert DBC[CP.carFingerprint][Bus.pt] == "hyundai_can_refresh_generated"
     assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_REFRESH_MSGS
 
+  def test_elantra_refresh_decodes_classic_media_buttons(self):
+    toggles = get_test_toggles()
+    CP = CarInterface.get_params(CAR.HYUNDAI_ELANTRA_HEV_2024, gen_empty_fingerprint(), [], True, False, False, toggles)
+    FPCP = CarInterface.get_starpilot_params(CAR.HYUNDAI_ELANTRA_HEV_2024, gen_empty_fingerprint(), [], CP, toggles)
+
+    car_state = CarState(CP, FPCP)
+    can_parsers = car_state.get_can_parsers(CP)
+    packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
+    media_msg = packer.make_can_msg("GW_SWRC_PE", 0, {"C_ModeSW": 1, "C_MTSSW": 1})
+
+    can_parsers[Bus.pt].update([(1_000_000_000, [media_msg])])
+    _, fp_ret = car_state.update(can_parsers, toggles)
+
+    assert fp_ret.modePressed
+    assert fp_ret.customPressed
+
   def test_hyundai_lkas_button_sets_starpilot_safety_flag(self):
     fingerprint = gen_empty_fingerprint()
     fingerprint[0][0x391] = 8

opendbc_repo/opendbc/car/nissan/interface.py

@@ -17,15 +17,12 @@ LEAF_ADAS_COMMAND_ADDRS = frozenset((0x1C3, 0x2B0))
 LEAF_2025_SV_PLUS_CAMERA_FW = b'6WK2CDB\x04\x18\x00\x00\x00\x00\x00R=1\x18\x99\x10\x00\x00\x00\x80'
 
 # This Leaf camera uses KWP2000 rather than UDS for session management.
-LEAF_KWP_AFTER_SALES_REQUEST = b"\x10\xC0"
-LEAF_KWP_AFTER_SALES_RESPONSE = b"\x50\xC0"
+LEAF_KWP_DATA_MONITOR_REQUEST = b"\x10\xF0"
+LEAF_KWP_DATA_MONITOR_RESPONSE = b"\x50\xF0"
 LEAF_KWP_DISABLE_NORMAL_TX = b"\x28\x01"
-LEAF_KWP_ENABLE_NORMAL_TX = b"\x29\x01"
-LEAF_KWP_ENABLE_NORMAL_TX_RESPONSE = b"\x69"
 
-LEAF_KWP_SESSIONS = (
-  (LEAF_KWP_AFTER_SALES_REQUEST, LEAF_KWP_AFTER_SALES_RESPONSE),
-  (NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_DIAGNOSTIC_RESPONSE_KWP),
+LEAF_KWP_TAKEOVER_SESSIONS = (
+  (LEAF_KWP_DATA_MONITOR_REQUEST, LEAF_KWP_DATA_MONITOR_RESPONSE),
 )
 
 
@@ -68,25 +65,45 @@ def leaf_adas_commands_silent(can_recv, settle_time=0.05, observe_time=0.15):
   return saw_adas_bus_traffic
 
 
+def leaf_adas_commands_present(can_recv, settle_time=0.05, observe_time=0.15):
+  """Confirm the stock ADAS command sender resumed on bus 1."""
+  if can_recv is None:
+    return False
+
+  try:
+    time.sleep(settle_time)
+    can_recv()
+
+    deadline = time.monotonic() + observe_time
+    while time.monotonic() < deadline:
+      for packet in can_recv(wait_for_one=True):
+        if any(msg.src == LEAF_ADAS_COMMAND_BUS and msg.address in LEAF_ADAS_COMMAND_ADDRS for msg in packet):
+          return True
+  except Exception as e:
+    ecu_log(f"Nissan Leaf ADAS TX recovery verification exception: {e}")
+    return False
+
+  ecu_log("Nissan Leaf ADAS normal TX recovery could not be verified")
+  return False
+
+
 def restore_leaf_adas_tx(can_recv, can_send):
-  """Re-enable normal KWP message transmission, trying both safe diagnostic sessions."""
+  """Return to the confirmed KWP default session and verify normal TX resumes."""
   if can_recv is None or can_send is None:
     return False
 
-  for diag_request, diag_response in LEAF_KWP_SESSIONS:
-    try:
-      ecu_log(f"Nissan Leaf ADAS TX restore using KWP session {diag_request.hex()}")
-      query = IsoTpParallelQuery(
-        can_send, can_recv, LEAF_ADAS_ECU_BUS, [LEAF_ADAS_ECU_ADDR],
-        [diag_request, LEAF_KWP_ENABLE_NORMAL_TX],
-        [diag_response, LEAF_KWP_ENABLE_NORMAL_TX_RESPONSE],
-        response_offset=NISSAN_RX_OFFSET,
-      )
-      if query.get_data(0.2):
-        ecu_log("Nissan Leaf ADAS normal TX restored")
-        return True
-    except Exception as e:
-      ecu_log(f"Nissan Leaf ADAS TX restore exception: {e}")
+  try:
+    ecu_log("Nissan Leaf ADAS TX restore using KWP default session 1081")
+    query = IsoTpParallelQuery(
+      can_send, can_recv, LEAF_ADAS_ECU_BUS, [LEAF_ADAS_ECU_ADDR],
+      [NISSAN_DIAGNOSTIC_REQUEST_KWP], [NISSAN_DIAGNOSTIC_RESPONSE_KWP],
+      response_offset=NISSAN_RX_OFFSET,
+    )
+    if query.get_data(0.2) and leaf_adas_commands_present(can_recv):
+      ecu_log("Nissan Leaf ADAS normal TX restored and command traffic confirmed")
+      return True
+  except Exception as e:
+    ecu_log(f"Nissan Leaf ADAS TX restore exception: {e}")
 
   ecu_log("Nissan Leaf ADAS normal TX restore was not confirmed")
   return False
@@ -141,10 +158,10 @@ class CarInterface(CarInterfaceBase):
     from openpilot.common.params import Params
     params = Params()
     ecu_disabled = False
-    for diag_request, diag_response in LEAF_KWP_SESSIONS:
+    for diag_request, diag_response in LEAF_KWP_TAKEOVER_SESSIONS:
       ecu_log(f"Nissan Leaf ADAS takeover using KWP session {diag_request.hex()}")
       ecu_disabled = disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR,
-                                 com_cont_req=LEAF_KWP_DISABLE_NORMAL_TX, require_response=True, retry=3,
+                                 com_cont_req=LEAF_KWP_DISABLE_NORMAL_TX, require_response=True, retry=1,
                                  diag_request=diag_request, diag_response=diag_response, response_offset=NISSAN_RX_OFFSET)
       if ecu_disabled:
         break

opendbc_repo/opendbc/car/nissan/tests/test_nissan.py

@@ -5,7 +5,8 @@ import pytest
 from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs
 from opendbc.car.can_definitions import CanData
 from opendbc.car.nissan.carstate import CarState
-from opendbc.car.nissan.interface import CarInterface, LEAF_2025_SV_PLUS_CAMERA_FW, leaf_adas_commands_silent, restore_leaf_adas_tx
+from opendbc.car.nissan.interface import CarInterface, LEAF_2025_SV_PLUS_CAMERA_FW, leaf_adas_commands_present, \
+                                          leaf_adas_commands_silent, restore_leaf_adas_tx
 from opendbc.car.nissan.values import CAR, CarControllerParams, NissanSafetyFlags
 
 
@@ -143,28 +144,24 @@ def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled):
   monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None)
   CarInterface.init(CP, None, None)
 
-  assert len(calls) == (1 if ecu_disabled else 2)
+  assert len(calls) == 1
   assert calls[0]["addr"] == 0x707
   assert calls[0]["bus"] == 0
   assert calls[0]["response_offset"] == 0x20
   assert calls[0]["require_response"] is True
-  assert calls[0]["diag_request"] == b"\x10\xc0"
-  assert calls[0]["diag_response"] == b"\x50\xc0"
+  assert calls[0]["diag_request"] == b"\x10\xf0"
+  assert calls[0]["diag_response"] == b"\x50\xf0"
   assert calls[0]["com_cont_req"] == b"\x28\x01"
-  assert calls[0]["retry"] == 3
-  if not ecu_disabled:
-    assert calls[1]["diag_request"] == b"\x10\x81"
-    assert calls[1]["diag_response"] == b"\x50\x81"
+  assert calls[0]["retry"] == 1
   assert CP.openpilotLongitudinalControl is ecu_disabled
   assert CP.pcmCruise is not ecu_disabled
   assert bool(CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL) is ecu_disabled
 
 
-def test_leaf_kwp_session_can_confirm_ecu_disable(monkeypatch):
+def test_leaf_kwp_data_monitor_session_can_confirm_ecu_disable(monkeypatch):
   CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
-  results = iter((False, True))
 
-  monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: next(results))
+  monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: True)
   monkeypatch.setattr("opendbc.car.nissan.interface.leaf_adas_commands_silent", lambda *_: True)
   monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None)
   CarInterface.init(CP, None, None)
@@ -205,7 +202,17 @@ def test_leaf_adas_command_silence_requires_live_bus_without_stock_commands(monk
   assert not leaf_adas_commands_silent(lambda wait_for_one=False: [], settle_time=0, observe_time=0.001)
 
 
-def test_leaf_adas_restore_uses_kwp_enable_normal_transmission(monkeypatch):
+def test_leaf_adas_command_recovery_requires_stock_command(monkeypatch):
+  monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None)
+
+  def stock_command_traffic(wait_for_one=False):
+    return [] if not wait_for_one else [[CanData(0x1C3, b"\x00" * 8, 1)]]
+
+  assert leaf_adas_commands_present(stock_command_traffic, settle_time=0, observe_time=0.001)
+  assert not leaf_adas_commands_present(lambda wait_for_one=False: [], settle_time=0, observe_time=0.001)
+
+
+def test_leaf_adas_restore_returns_to_kwp_default_session(monkeypatch):
   queries = []
 
   class FakeQuery:
@@ -216,7 +223,8 @@ def test_leaf_adas_restore_uses_kwp_enable_normal_transmission(monkeypatch):
       return {(0x707, None): b""}
 
   monkeypatch.setattr("opendbc.car.nissan.interface.IsoTpParallelQuery", FakeQuery)
+  monkeypatch.setattr("opendbc.car.nissan.interface.leaf_adas_commands_present", lambda *_: True)
   monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None)
 
   assert restore_leaf_adas_tx(lambda **kwargs: [], lambda msgs: None)
-  assert queries == [(0, [0x707], [b"\x10\xc0", b"\x29\x01"], [b"\x50\xc0", b"\x69"], 0x20)]
+  assert queries == [(0, [0x707], [b"\x10\x81"], [b"\x50\x81"], 0x20)]

panda/board/obj/gitversion.h

@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-21805a54-DEBUG";
+const uint8_t gitversion[19] = "DEV-05331a78-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-21805a54-DEBUG
+DEV-05331a78-DEBUG

selfdrive/car/card.py

@@ -24,7 +24,7 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
 from openpilot.common.constants import CV
 from openpilot.selfdrive.car.cruise import (
   VCruiseHelper, IMPERIAL_INCREMENT, V_CRUISE_MAX, V_CRUISE_MIN,
-  is_speed_limit_confirmation_pending,
+  is_csc_override_pending, is_speed_limit_confirmation_pending,
 )
 from openpilot.selfdrive.car.redneck_cruise import RedneckCruise, select_redneck_target_speed
 from openpilot.selfdrive.car.car_specific import MockCarState
@@ -277,6 +277,7 @@ class Car:
     )
     if not preap_software_cruise:
       speed_limit_confirmation_pending = is_speed_limit_confirmation_pending(self.sm['starpilotPlan'])
+      csc_override_pending = is_csc_override_pending(self.sm['starpilotPlan'])
       self.v_cruise_helper.update_v_cruise(
         CS,
         self.sm['carControl'].enabled,
@@ -284,6 +285,7 @@ class Car:
         speed_limit_confirmation_pending,
         self.starpilot_toggles,
         FPCS,
+        csc_active=csc_override_pending,
       )
     else:
       preap_v_cruise_kph = float(CS.cruiseState.speed * CV.MS_TO_KPH)

selfdrive/car/cruise.py

@@ -37,6 +37,11 @@ def is_speed_limit_confirmation_pending(starpilot_plan) -> bool:
   return bool(starpilot_plan.speedLimitChanged and starpilot_plan.unconfirmedSlcSpeedLimit >= 1)
 
 
+def is_csc_override_pending(starpilot_plan) -> bool:
+  """Accel presses cancel an active curve slowdown instead of raising the set speed."""
+  return bool(starpilot_plan.cscControllingSpeed) and not is_speed_limit_confirmation_pending(starpilot_plan)
+
+
 class VCruiseHelper:
   def __init__(self, CP, FPCP=None):
     self.CP = CP
@@ -89,13 +94,13 @@ class VCruiseHelper:
       return bool(getattr(starpilot_car_state, "decelHardCruise", False))
     return False
 
-  def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None):
+  def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None, csc_active=False):
     self.v_cruise_kph_last = self.v_cruise_kph
 
     if CS.cruiseState.available:
       if self.gm_cc_only or self.redneck_non_pcm or not self.CP.pcmCruise:
         # if stock cruise is completely disabled, then we can use our own set speed logic
-        self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state)
+        self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state, csc_active)
         self.v_cruise_cluster_kph = self.v_cruise_kph
         self.update_button_timers(CS, enabled, starpilot_car_state)
       else:
@@ -111,7 +116,7 @@ class VCruiseHelper:
       self.v_cruise_kph = V_CRUISE_UNSET
       self.v_cruise_cluster_kph = V_CRUISE_UNSET
 
-  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None):
+  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None, csc_active=False):
     # handle button presses. TODO: this should be in state_control, but a decelCruise press
     # would have the effect of both enabling and changing speed is checked after the state transition
     if not enabled:
@@ -126,7 +131,9 @@ class VCruiseHelper:
     for b in CS.buttonEvents:
       event_button_type = b.type.raw
       if event_button_type in self.button_timers:
-        if speed_limit_changed and b.pressed:
+        # decel presses keep their normal meaning
+        consume_press = speed_limit_changed or (csc_active and event_button_type in ACCEL_CRUISE_BUTTONS)
+        if consume_press and b.pressed:
           self.confirmation_button_suppressed.add(event_button_type)
         elif not b.pressed and event_button_type in self.confirmation_button_suppressed:
           self.confirmation_button_suppressed.remove(event_button_type)

selfdrive/car/tests/test_csc_cruise_button.py

@@ -0,0 +1,74 @@
+import pytest
+
+from types import SimpleNamespace
+
+from openpilot.selfdrive.car.cruise import ButtonType, VCruiseHelper, is_csc_override_pending
+
+
+def make_helper():
+  CP = SimpleNamespace(carFingerprint="MOCK", flags=0, pcmCruise=False, brand="mock")
+  helper = VCruiseHelper(CP)
+  helper.v_cruise_kph = 40.0
+  return helper
+
+
+def make_toggles():
+  return SimpleNamespace(cruise_increase=1.0, cruise_increase_long=5.0, reverse_cruise_increase=False)
+
+
+def make_cs(button_events):
+  return SimpleNamespace(
+    buttonEvents=button_events,
+    cruiseState=SimpleNamespace(available=True, standstill=False, speed=0, speedCluster=0),
+    gasPressed=False,
+    vEgo=20.0,
+  )
+
+
+def press(button):
+  return SimpleNamespace(type=SimpleNamespace(raw=button), pressed=True)
+
+
+def release(button):
+  return SimpleNamespace(type=SimpleNamespace(raw=button), pressed=False)
+
+
+def press_and_release(helper, button, csc_active):
+  toggles = make_toggles()
+  helper.update_v_cruise(make_cs([press(button)]), True, True, False, toggles, None, csc_active=csc_active)
+  helper.update_v_cruise(make_cs([release(button)]), True, True, False, toggles, None, csc_active=csc_active)
+
+
+def test_accel_press_consumed_while_csc_active():
+  helper = make_helper()
+
+  press_and_release(helper, ButtonType.accelCruise, csc_active=True)
+
+  assert helper.v_cruise_kph == pytest.approx(40.0)
+
+
+def test_accel_press_adjusts_set_speed_when_csc_inactive():
+  helper = make_helper()
+
+  press_and_release(helper, ButtonType.accelCruise, csc_active=False)
+
+  assert helper.v_cruise_kph > 40.0
+
+
+def test_decel_press_still_works_while_csc_active():
+  helper = make_helper()
+
+  press_and_release(helper, ButtonType.decelCruise, csc_active=True)
+
+  assert helper.v_cruise_kph < 40.0
+
+
+def test_csc_override_pending_defers_to_slc_confirmation():
+  active_plan = SimpleNamespace(cscControllingSpeed=True, speedLimitChanged=False, unconfirmedSlcSpeedLimit=0)
+  assert is_csc_override_pending(active_plan)
+
+  idle_plan = SimpleNamespace(cscControllingSpeed=False, speedLimitChanged=False, unconfirmedSlcSpeedLimit=0)
+  assert not is_csc_override_pending(idle_plan)
+
+  slc_pending_plan = SimpleNamespace(cscControllingSpeed=True, speedLimitChanged=True, unconfirmedSlcSpeedLimit=25)
+  assert not is_csc_override_pending(slc_pending_plan)

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -709,7 +709,7 @@ IONIQ_5_LOW_SPEED_CENTER_LAT = 0.40
 IONIQ_5_LOW_SPEED_CENTER_LAT_WIDTH = 0.10
 IONIQ_5_LOW_SPEED_CENTER_JERK = 0.40
 IONIQ_5_LOW_SPEED_CENTER_JERK_WIDTH = 0.12
-IONIQ_5_FRICTION_JERK_DEADZONE_MAX = 0.30
+IONIQ_5_FRICTION_JERK_DEADZONE_MAX = 0.36
 IONIQ_5_FRICTION_JERK_DEADZONE_LAT = 1.25
 IONIQ_5_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.35
 IONIQ_5_FRICTION_JERK_DEADZONE_SPEED = 18.0
@@ -866,7 +866,7 @@ IONIQ_6_CURVY_TURN_IN_TRIM_LAT_START = 1.0
 IONIQ_6_CURVY_TURN_IN_TRIM_LAT_END = 2.5
 IONIQ_6_CURVY_TURN_IN_TRIM_LAT_ONSET_WIDTH = 0.18
 IONIQ_6_CURVY_TURN_IN_TRIM_LAT_CUTOFF_WIDTH = 0.30
-IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX = 0.18
+IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX = 0.24
 IONIQ_6_2023_UNWIND_FF_OVERSHOOT = 0.15
 IONIQ_6_2023_UNWIND_FF_OVERSHOOT_WIDTH = 0.18
 IONIQ_6_2023_UNWIND_FF_JERK = 0.10

selfdrive/controls/lib/lead_follow_policy.py

@@ -28,6 +28,9 @@ FOLLOW_TRANSITION_MIN_TTC = 6.0
 FOLLOW_HEADWAY_MARGIN = 0.90
 FOLLOW_SIGN_CROSS_STEP = 0.10
 FOLLOW_TRANSITION_MAX_BRAKE = 0.25
+FOLLOW_STEADY_DEADBAND_MAX_TARGET = 0.28
+FOLLOW_STEADY_DEADBAND_MAX_CLOSING = 0.75
+FOLLOW_STEADY_DEADBAND_MAX_HEADWAY_MARGIN = 0.90
 
 
 @dataclass(frozen=True)
@@ -184,6 +187,25 @@ def _transition_target(lead, v_ego: float, t_follow: float, previous: float, tar
   return limited if abs(limited - float(target)) > 1e-6 else None
 
 
+def _steady_follow_deadband(lead, v_ego: float, t_follow: float, previous: float, target: float) -> float:
+  """Remove only small sign reversals in an already matched, non-urgent follow."""
+  if not _matched(lead, v_ego, t_follow):
+    return float(target)
+  if float(lead.vLead) - float(v_ego) > 0.15:
+    return float(target)
+  if max(0.0, float(v_ego) - float(lead.vLead)) > FOLLOW_STEADY_DEADBAND_MAX_CLOSING:
+    return float(target)
+  if _headway(lead, v_ego) - float(t_follow) > FOLLOW_STEADY_DEADBAND_MAX_HEADWAY_MARGIN:
+    return float(target)
+  if (
+    float(previous) * float(target) < 0.0 and
+    abs(float(previous)) <= FOLLOW_STEADY_DEADBAND_MAX_TARGET and
+    abs(float(target)) <= FOLLOW_STEADY_DEADBAND_MAX_TARGET
+  ):
+    return 0.0
+  return float(target)
+
+
 def apply(lead_one, lead_two, *, source: str, active: bool, v_ego: float, t_follow: float,
           previous_target: float, raw_target: float, tracking: bool, post_departure: bool,
           blocked: bool, panic_bypass: bool) -> FollowResult:
@@ -202,7 +224,11 @@ def apply(lead_one, lead_two, *, source: str, active: bool, v_ego: float, t_foll
     target = max(target, floor)
   if cap is not None:
     target = min(target, cap)
-  transition = _transition_target(lead, v_ego, t_follow, previous_target, target)
-  if transition is not None:
-    target = transition
+  deadband_target = _steady_follow_deadband(lead, v_ego, t_follow, previous_target, target)
+  if deadband_target != target:
+    target = deadband_target
+  else:
+    transition = _transition_target(lead, v_ego, t_follow, previous_target, target)
+    if transition is not None:
+      target = transition
   return FollowResult(lead, cap, floor, target)

selfdrive/controls/lib/longcontrol_vehicle_tunes.py

@@ -27,11 +27,11 @@ GM_TRUCK_TARGET_FILTER_DROP_BYPASS = 0.45
 TOYOTA_SIENNA_TARGET_FILTER_MIN_SPEED = 12.0
 TOYOTA_SIENNA_TARGET_FILTER_UP_TAU = 0.32
 TOYOTA_SIENNA_TARGET_FILTER_DOWN_TAU = 0.24
-TOYOTA_SIENNA_LOW_SPEED_ACCEL_UP_TAU = 0.35
+TOYOTA_SIENNA_LOW_SPEED_ACCEL_UP_TAU = 0.50
 TOYOTA_SIENNA_TARGET_FILTER_BRAKE_BYPASS = -0.75
 TOYOTA_SIENNA_TARGET_FILTER_DROP_BYPASS = 0.65
-TOYOTA_SIENNA_COMFORT_FILTER_MIN_SPEED = 5.0
-TOYOTA_SIENNA_COMFORT_FILTER_MIN_DISTANCE = 10.0
+TOYOTA_SIENNA_COMFORT_FILTER_MIN_SPEED = 1.0
+TOYOTA_SIENNA_COMFORT_FILTER_MIN_DISTANCE = 7.0
 TOYOTA_SIENNA_COMFORT_FILTER_MIN_TTC = 4.5
 TOYOTA_SIENNA_COMFORT_FILTER_MAX_CLOSING_SPEED = 4.0
 TOYOTA_SIENNA_COMFORT_FILTER_MAX_LEAD_BRAKE = 2.5
@@ -295,7 +295,9 @@ class LongControlVehicleTuning:
 
     if v_ego < TOYOTA_SIENNA_TARGET_FILTER_MIN_SPEED and not comfort_filter_active:
       if a_target > 0.0:
-        if not self.toyota_sienna_target_filter_initialized or self.toyota_sienna_filtered_a_target < 0.0:
+        if not self.toyota_sienna_target_filter_initialized or (
+          self.toyota_sienna_filtered_a_target < 0.0 and comfort_lead is None
+        ):
           self.toyota_sienna_filtered_a_target = 0.0
           self.toyota_sienna_target_filter_initialized = True
         alpha = DT_CTRL / (TOYOTA_SIENNA_LOW_SPEED_ACCEL_UP_TAU + DT_CTRL)
@@ -304,6 +306,11 @@ class LongControlVehicleTuning:
         )
         return self.toyota_sienna_filtered_a_target
 
+      if comfort_lead is not None:
+        self.toyota_sienna_filtered_a_target = float(a_target)
+        self.toyota_sienna_target_filter_initialized = True
+        return float(a_target)
+
       self.toyota_sienna_target_filter_initialized = False
       return a_target
 

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -138,6 +138,8 @@ LEAD_ACCEL_TAU = 1.5
 FCW_MIN_MODEL_PROB = 0.9
 FCW_MIN_CLOSING_SPEED = 0.5
 FCW_MAX_TTC = 4.0
+MODEL_LEAD_TRAJECTORY_MAX_LEAD_BRAKE = 0.5
+MODEL_LEAD_TRAJECTORY_MAX_CLOSING_TTC = 7.0
 
 
 # Fewer timestamps don't hurt performance and lead to
@@ -178,6 +180,17 @@ def build_model_lead_trajectory(model_lead, radar_lead, v_ego):
   if not np.isfinite(raw_d_rel) or not np.isfinite(raw_v_lead):
     return None
 
+  # The model path is a comfort prediction, not the raw safety measurement.
+  # When the measured lead is already braking or the gap is closing quickly,
+  # keep the legacy raw-lead path so an optimistic model horizon cannot delay
+  # the first braking response.
+  raw_lead_brake = max(0.0, -float(getattr(radar_lead, "aLeadK", 0.0)))
+  closing_speed = max(0.0, float(v_ego) - raw_v_lead)
+  ttc = raw_d_rel / max(closing_speed, 1e-3) if closing_speed > 0.1 else float("inf")
+  if (raw_lead_brake > MODEL_LEAD_TRAJECTORY_MAX_LEAD_BRAKE or
+      (closing_speed > 0.75 and ttc < MODEL_LEAD_TRAJECTORY_MAX_CLOSING_TTC)):
+    return None
+
   # The model contributes future deltas only. This preserves raw lead source
   # selection and keeps the current lead distance/speed safety anchor intact.
   x_lead_traj = raw_d_rel + (model_x - model_x[0])

selfdrive/controls/tests/test_conditional_experimental_mode.py

@@ -1,6 +1,8 @@
 from pathlib import Path
 from types import SimpleNamespace
 
+import numpy as np
+
 from openpilot.common.constants import CV
 from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner
 import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module
@@ -1168,6 +1170,7 @@ def test_starpilot_planner_updates_cem_with_current_frame_state(monkeypatch):
 
   try:
     monkeypatch.setattr(starpilot_planner_module, "calculate_road_curvature", lambda model, v_ego: (0.01, 1.0))
+    monkeypatch.setattr(starpilot_planner_module, "extract_curve_profile", lambda model: (np.zeros(33), np.zeros(33)))
     monkeypatch.setattr(planner.starpilot_acceleration, "update", lambda *args, **kwargs: None)
     monkeypatch.setattr(planner.starpilot_events, "update", lambda *args, **kwargs: None)
     monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: 0.0)

selfdrive/controls/tests/test_curve_speed_controller.py

@@ -0,0 +1,507 @@
+import numpy as np
+import pytest
+
+from types import SimpleNamespace
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.starpilot.common.starpilot_variables import DEFAULT_LATERAL_ACCELERATION
+from openpilot.starpilot.controls.lib.curve_speed_controller import (
+  CSC_APPROACH_DECEL,
+  CSC_COMFORT_MARGIN,
+  CSC_COUNT_CAP,
+  CSC_EGO_HEADROOM,
+  CSC_LAT_ACCEL_MAX,
+  CSC_MIN_SPEED,
+  CSC_NUDGE,
+  CSC_NUDGE_WEIGHT,
+  CSC_OVERRIDE_WATCH_TIME,
+  CSC_TARGET_UP_RATE,
+  CSC_TRAINING_SETTLE_TIME,
+  CurveSpeedController,
+  weighted_isotonic,
+)
+
+
+class FakeParams:
+  def __init__(self, values=None):
+    self.values = dict(values or {})
+
+  def get(self, *args, **kwargs):
+    key = args[0] if args else None
+    return self.values.get(key)
+
+  def put_nonblocking(self, key, value):
+    self.values[key] = value
+
+
+def make_controller(curve_profile=None, curvature_data=None, weather_id=0, reduce_lat=0.0, road_curvature=0.02, driving_in_curve=False):
+  if curve_profile is None:
+    curve_profile = (np.zeros(33), np.linspace(0.0, 300.0, 33))
+
+  planner = SimpleNamespace(
+    params=FakeParams({"CurvatureData": curvature_data} if curvature_data is not None else None),
+    curve_profile=curve_profile,
+    starpilot_weather=SimpleNamespace(weather_id=weather_id, reduce_lateral_acceleration=reduce_lat),
+    road_curvature=road_curvature,
+    driving_in_curve=driving_in_curve,
+    tracking_lead=False,
+    lateral_acceleration=0.0,
+  )
+  controller = CurveSpeedController(SimpleNamespace(starpilot_planner=planner))
+  return planner, controller
+
+
+def make_sm(*, gas=False, brake=False, long_active=True, blinker=False, accel_pressed=False):
+  return {
+    "carControl": SimpleNamespace(longActive=long_active),
+    "carState": SimpleNamespace(gasPressed=gas, brakePressed=brake, leftBlinker=blinker, rightBlinker=False),
+    "starpilotCarState": SimpleNamespace(accelPressed=accel_pressed),
+    "onroadEvents": [],
+  }
+
+
+def single_apex_profile(curvature, distance):
+  distances = np.linspace(0.0, max(distance * 1.5, 1.0), 33)
+  curvatures = np.zeros(33)
+  index = int(np.argmin(np.abs(distances - distance)))
+  distances[index] = distance
+  curvatures[index] = curvature
+  return curvatures, distances
+
+
+def converge(controller, v_ego, v_cruise, frames=600):
+  for _ in range(frames):
+    controller.update_target(v_ego, v_cruise)
+  return controller.target
+
+
+def envelope_speed(controller, curvature, distance):
+  curve_speed = max(float(np.sqrt(controller.lat_accel_for_curvature(curvature) / curvature)), CSC_MIN_SPEED)
+  return float(np.sqrt(curve_speed**2 + 2.0 * CSC_APPROACH_DECEL * distance))
+
+
+def test_straight_road_target_is_cruise_speed():
+  _, controller = make_controller()
+
+  controller.update_target(30.0, 30.0)
+
+  assert controller.target == pytest.approx(30.0)
+
+
+def test_distant_apex_does_not_constrain_until_braking_is_due():
+  # derived from the shipped decel so retuning it doesn't silently invalidate the case
+  _, probe = make_controller()
+  curve_speed = max(float(np.sqrt(probe.lat_accel_for_curvature(0.02) / 0.02)), CSC_MIN_SPEED)
+  beyond_braking = 1.3 * (30.0**2 - curve_speed**2) / (2 * CSC_APPROACH_DECEL)
+
+  _, controller = make_controller(curve_profile=single_apex_profile(0.02, beyond_braking))
+
+  target = converge(controller, 30.0, 30.0)
+
+  assert target == pytest.approx(30.0)
+
+
+def test_apex_in_braking_range_constrains_to_kinematic_envelope():
+  _, controller = make_controller(curve_profile=single_apex_profile(0.02, 150.0))
+
+  target = converge(controller, 30.0, 30.0)
+
+  assert target == pytest.approx(envelope_speed(controller, 0.02, 150.0), abs=0.1)
+  assert target < 30.0
+
+
+def test_exit_recovery_rises_immediately_without_freeze():
+  planner, controller = make_controller(curve_profile=single_apex_profile(0.03, 20.0))
+  low_target = converge(controller, 15.0, 30.0)
+  assert low_target < 20.0
+
+  planner.curve_profile = (np.zeros(33), np.linspace(0.0, 300.0, 33))
+  controller.update_target(15.0, 30.0)
+  assert controller.target > low_target          # rises on the very next frame, no freeze
+  assert controller.target - low_target == pytest.approx(CSC_TARGET_UP_RATE * DT_MDL)
+
+  # and it clears the car by the headroom within the time the up-rate needs
+  frames = int((15.0 + CSC_EGO_HEADROOM - controller.target) / (CSC_TARGET_UP_RATE * DT_MDL)) + 1
+  for _ in range(frames):
+    controller.update_target(15.0, 30.0)
+  assert controller.target >= 15.0 + CSC_EGO_HEADROOM
+
+  recovered = converge(controller, 15.0, 30.0)
+  assert recovered == pytest.approx(30.0)
+
+
+def test_upward_jitter_in_the_envelope_is_rate_limited():
+  # a sweeper the envelope only grazes: raw_target flicks between a mild cap and the
+  # set speed. The target must not chase the jumps, or the glow strobes.
+  planner, controller = make_controller(curve_profile=single_apex_profile(0.002, 40.0))
+  steady = converge(controller, 30.0, 32.0)
+  assert steady < 32.0
+
+  flat = (np.zeros(33), np.linspace(0.0, 300.0, 33))
+  grazing = planner.curve_profile
+  peak = steady
+  for i in range(40):
+    planner.curve_profile = flat if i % 2 else grazing
+    controller.update_target(30.0, 32.0)
+    assert controller.target - peak <= CSC_TARGET_UP_RATE * DT_MDL + 1e-6
+    peak = controller.target
+
+
+def test_fresh_activation_seeds_at_envelope_not_cruise():
+  _, controller = make_controller(curve_profile=(np.full(33, 0.05), np.linspace(0.0, 60.0, 33)))
+
+  controller.update_target(6.0, 30.0)
+
+  assert controller.target < 15.0
+
+
+def test_target_never_trails_accelerating_car_when_unconstrained():
+  planner, controller = make_controller(curve_profile=single_apex_profile(0.03, 20.0))
+  converge(controller, 15.0, 30.0)
+
+  planner.curve_profile = (np.zeros(33), np.linspace(0.0, 300.0, 33))
+  v_ego = 15.0
+  caught_up = None
+  for frame in range(200):
+    v_ego = min(v_ego + 2.0 * DT_MDL, 30.0)
+    controller.update_target(v_ego, 30.0)
+    # the target climbs faster than the car can, so once it is ahead it stays ahead
+    if controller.target >= v_ego:
+      caught_up = caught_up if caught_up is not None else frame
+    assert caught_up is None or controller.target >= min(30.0, v_ego) - 1e-6
+
+  assert caught_up is not None and caught_up * DT_MDL < 2.0
+  assert controller.target == pytest.approx(30.0)
+
+
+def test_target_does_not_ratchet_down_with_ego_speed():
+  _, controller = make_controller(curve_profile=single_apex_profile(0.02, 150.0))
+  target = converge(controller, 30.0, 30.0)
+  assert target > 15.0
+
+  controller.update_target(14.0, 30.0)
+
+  assert controller.target == pytest.approx(target, abs=0.2)
+
+
+def test_sharp_curve_target_floors_at_min_speed():
+  _, controller = make_controller(curve_profile=(np.full(33, 0.1), np.linspace(0.0, 100.0, 33)))
+
+  target = converge(controller, 15.0, 30.0)
+
+  assert target == pytest.approx(CSC_MIN_SPEED, abs=0.05)
+
+
+def test_weather_reduces_curve_speed():
+  _, dry = make_controller(curve_profile=single_apex_profile(0.01, 0.0))
+  _, wet = make_controller(curve_profile=single_apex_profile(0.01, 0.0), weather_id=1, reduce_lat=0.2)
+
+  dry_target = converge(dry, 20.0, 30.0)
+  wet_target = converge(wet, 20.0, 30.0)
+
+  assert wet_target < dry_target
+  assert wet_target == pytest.approx(dry_target * np.sqrt(0.8), abs=0.1)
+
+
+def test_prior_gives_higher_lat_accel_for_sharper_curves():
+  _, controller = make_controller()
+
+  assert controller.learned_lat_accel(0.001) == pytest.approx(1.5, abs=0.05)
+  assert controller.learned_lat_accel(0.1) == pytest.approx(2.9, abs=0.05)
+  assert controller.lateral_acceleration == pytest.approx(DEFAULT_LATERAL_ACCELERATION)
+
+
+def test_comfort_margin_aims_below_the_learned_habit():
+  _, controller = make_controller()
+
+  assert controller.lat_accel_for_curvature(0.01) == pytest.approx(
+    controller.learned_lat_accel(0.01) * CSC_COMFORT_MARGIN)
+  assert controller.lat_accel_for_curvature(0.01) < controller.learned_lat_accel(0.01)
+
+
+def test_margin_slider_overrides_the_default():
+  _, controller = make_controller()
+  controller.starpilot_toggles = SimpleNamespace(csc_margin=0.7)
+
+  assert controller.comfort_margin == pytest.approx(0.7)
+  assert controller.lat_accel_for_curvature(0.01) == pytest.approx(
+    controller.learned_lat_accel(0.01) * 0.7)
+
+
+def test_margin_falls_back_to_default_without_toggles():
+  _, controller = make_controller()
+
+  # toggles are only attached once vcruise runs
+  assert controller.comfort_margin == pytest.approx(CSC_COMFORT_MARGIN)
+  controller.starpilot_toggles = SimpleNamespace(csc_margin=0.0)
+  assert controller.comfort_margin == pytest.approx(CSC_COMFORT_MARGIN)
+
+
+def test_lower_margin_engages_on_gentler_curves():
+  planner, controller = make_controller(curve_profile=single_apex_profile(0.004, 0.0))
+  controller.starpilot_toggles = SimpleNamespace(csc_margin=1.0)
+  relaxed = converge(controller, 30.0, 30.0)
+
+  planner2, aggressive_controller = make_controller(curve_profile=single_apex_profile(0.004, 0.0))
+  aggressive_controller.starpilot_toggles = SimpleNamespace(csc_margin=0.7)
+  aggressive = converge(aggressive_controller, 30.0, 30.0)
+
+  assert aggressive < relaxed
+
+
+def test_approach_decel_slider_moves_where_braking_starts():
+  # the same curve must bind further out when the approach is planned gentler
+  def bind_distance(decel):
+    planner, controller = make_controller(curve_profile=single_apex_profile(0.01, 200.0))
+    controller.starpilot_toggles = SimpleNamespace(csc_approach_decel=decel)
+    converge(controller, 30.0, 30.0)
+    return controller.target
+
+  assert bind_distance(0.3) < bind_distance(1.5)
+
+
+def test_approach_decel_falls_back_to_default():
+  _, controller = make_controller()
+
+  assert controller.approach_decel == pytest.approx(CSC_APPROACH_DECEL)
+  controller.starpilot_toggles = SimpleNamespace(csc_approach_decel=0.0)
+  assert controller.approach_decel == pytest.approx(CSC_APPROACH_DECEL)
+
+
+def test_binding_distance_reports_the_constraining_point():
+  _, controller = make_controller(curve_profile=single_apex_profile(0.02, 150.0))
+  converge(controller, 30.0, 30.0)
+
+  assert controller.binding_distance == pytest.approx(150.0, abs=1.0)
+
+
+def test_binding_distance_is_zero_when_unconstrained():
+  _, controller = make_controller()
+  converge(controller, 30.0, 30.0)
+
+  assert controller.binding_distance == 0.0
+
+
+def test_heavily_sampled_bucket_dominates_prior():
+  _, controller = make_controller(curvature_data={"0.05": {"average": 3.0, "count": 100000}})
+
+  assert controller.learned_lat_accel(0.05) == pytest.approx(3.0, abs=0.05)
+  assert controller.learned_lat_accel(0.08) >= controller.learned_lat_accel(0.05)
+
+
+def test_learned_curve_stays_monotonic_despite_low_outlier_bucket():
+  _, controller = make_controller(curvature_data={"0.05": {"average": 0.5, "count": 100000}})
+
+  assert controller.learned_lat_accel(0.05) >= controller.learned_lat_accel(0.03)
+
+
+def test_dense_bucket_is_not_overridden_by_sparse_neighbour():
+  # real device data: a running maximum ratcheted the 80-sample bucket up to the 20-sample neighbour
+  _, controller = make_controller(curvature_data={
+    "0.003": {"average": 1.95, "count": 20},
+    "0.005": {"average": 1.38, "count": 80},
+  })
+
+  assert controller.learned_lat_accel(0.005) < 1.82
+  assert controller.learned_lat_accel(0.005) >= controller.learned_lat_accel(0.003)
+
+
+def test_weighted_isotonic_pools_violators_by_weight():
+  fitted = weighted_isotonic(np.array([1.0, 3.0, 1.2]), np.array([1.0, 1.0, 1000.0]))
+
+  assert np.all(np.diff(fitted) >= -1e-9)
+  assert fitted[-1] == pytest.approx(1.2, abs=0.02)
+
+
+def test_weighted_isotonic_leaves_sorted_input_untouched():
+  values = np.array([1.0, 1.5, 2.0, 2.5])
+  fitted = weighted_isotonic(values, np.ones(4))
+
+  assert fitted == pytest.approx(values)
+
+
+def test_legacy_off_grid_curvature_data_merges_into_buckets():
+  _, controller = make_controller(curvature_data={
+    "0.0203": {"average": 2.5, "count": 10},
+    "0.02": {"average": 2.0, "count": 10},
+  })
+
+  assert controller.curvature_data["0.02"]["count"] == 20
+  assert controller.curvature_data["0.02"]["average"] == pytest.approx(2.25)
+
+
+def test_training_update_step_is_capped_by_ema_count():
+  planner, controller = make_controller(curvature_data={"0.02": {"average": 2.0, "count": 10000}}, driving_in_curve=True)
+  planner.lateral_acceleration = 3.0
+  controller.training_timer = CSC_TRAINING_SETTLE_TIME
+
+  controller.log_data(10.0, make_sm(long_active=False))
+
+  data = controller.curvature_data["0.02"]
+  assert data["count"] == 10001
+  assert data["average"] == pytest.approx((2.0 * CSC_COUNT_CAP + 3.0) / (CSC_COUNT_CAP + 1))
+
+
+def test_no_passive_training_right_after_csc_limited_speed():
+  planner, controller = make_controller(curve_profile=single_apex_profile(0.03, 20.0), driving_in_curve=True)
+  planner.lateral_acceleration = 3.0
+  converge(controller, 15.0, 30.0)
+  assert controller.training_quiet_timer > 0.0
+
+  controller.training_timer = CSC_TRAINING_SETTLE_TIME
+  controller.log_data(10.0, make_sm(long_active=False))
+  assert "0.02" not in controller.curvature_data
+  assert not controller.enable_training
+
+  controller.training_quiet_timer = 0.0
+  controller.training_timer = CSC_TRAINING_SETTLE_TIME
+  controller.log_data(10.0, make_sm(long_active=False))
+  assert controller.curvature_data["0.02"]["count"] == 1
+
+
+def test_training_settles_within_a_couple_of_seconds():
+  # a real drive rarely holds every eligibility condition for a whole model horizon,
+  # so the settle time has to be short enough that ordinary curves still teach it
+  planner, controller = make_controller(driving_in_curve=True)
+  planner.lateral_acceleration = 2.4
+  sm = make_sm(long_active=False)
+
+  for _ in range(int(CSC_TRAINING_SETTLE_TIME / DT_MDL) - 2):
+    controller.log_data(10.0, sm)
+  assert "0.02" not in controller.curvature_data
+
+  for _ in range(3):
+    controller.log_data(10.0, sm)
+  assert controller.curvature_data["0.02"]["count"] >= 1
+
+
+def test_brief_ineligibility_does_not_restart_the_settle_timer():
+  planner, controller = make_controller(driving_in_curve=True)
+  planner.lateral_acceleration = 2.4
+  sm = make_sm(long_active=False)
+  for _ in range(int(CSC_TRAINING_SETTLE_TIME / DT_MDL) + 1):
+    controller.log_data(10.0, sm)
+  trained = controller.curvature_data["0.02"]["count"]
+
+  # a lead flickers into the tracker for two frames, then leaves
+  planner.tracking_lead = True
+  controller.log_data(10.0, sm)
+  controller.log_data(10.0, sm)
+  planner.tracking_lead = False
+
+  controller.log_data(10.0, sm)
+  assert controller.curvature_data["0.02"]["count"] == trained + 1
+
+
+def test_sustained_ineligibility_still_drains_the_settle_timer():
+  planner, controller = make_controller(driving_in_curve=True)
+  planner.lateral_acceleration = 2.4
+  engaged = make_sm(long_active=True)
+  manual = make_sm(long_active=False)
+  for _ in range(int(CSC_TRAINING_SETTLE_TIME / DT_MDL) + 1):
+    controller.log_data(10.0, manual)
+
+  for _ in range(int(2 * CSC_TRAINING_SETTLE_TIME / DT_MDL)):
+    controller.log_data(10.0, engaged)
+  assert controller.training_timer == pytest.approx(0.0)
+
+  controller.log_data(10.0, manual)
+  assert not controller.enable_training
+
+
+def settle_override(controller, sm=None, frames=None):
+  """Run the post-override watch out so the pseudo-sample is committed."""
+  sm = sm if sm is not None else make_sm()
+  for _ in range(frames if frames is not None else int(CSC_OVERRIDE_WATCH_TIME / DT_MDL) + 1):
+    controller.handle_override(20.0, False, sm)
+
+
+def test_gas_override_nudges_bucket_up_once_per_episode():
+  _, controller = make_controller()
+  prior = controller.learned_lat_accel(0.02)
+  controller.target = 10.0
+
+  controller.handle_override(20.0, True, make_sm(gas=True))
+  controller.handle_override(20.0, True, make_sm(gas=True))
+  assert "0.02" not in controller.curvature_data  # still watching what the driver holds
+
+  settle_override(controller)
+  assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT
+  assert controller.curvature_data["0.02"]["average"] > prior
+
+  controller.handle_override(20.0, False, make_sm())
+  controller.target = 10.0
+  controller.handle_override(20.0, True, make_sm(gas=True))
+  settle_override(controller)
+  assert controller.curvature_data["0.02"]["count"] == 2 * CSC_NUDGE_WEIGHT
+
+
+def test_override_learns_the_cornering_the_driver_actually_held():
+  # the whole point: a fixed step needs several rejections to close a real disagreement,
+  # so record what they demonstrated instead
+  planner, observed = make_controller(driving_in_curve=True)
+  observed.target = 10.0
+  observed.handle_override(20.0, True, make_sm(gas=True))
+  planner.lateral_acceleration = 2.9          # they hold the curve much harder than CSC wanted
+  settle_override(observed, make_sm(gas=True))
+
+  _, stepped = make_controller(driving_in_curve=True)
+  stepped._apply_nudge(CSC_NUDGE)             # what the old fixed-step path would have recorded
+
+  assert observed.curvature_data["0.02"]["average"] == pytest.approx(2.9)
+  assert observed.curvature_data["0.02"]["average"] > stepped.curvature_data["0.02"]["average"]
+  assert observed.learned_lat_accel(0.02) > stepped.learned_lat_accel(0.02)
+
+
+def test_override_on_a_straight_still_registers_the_fixed_step():
+  planner, controller = make_controller()
+  prior = controller.learned_lat_accel(0.02)
+  controller.target = 10.0
+
+  controller.handle_override(20.0, True, make_sm(gas=True))
+  planner.lateral_acceleration = 0.0          # never reached a corner
+  settle_override(controller)
+
+  assert controller.curvature_data["0.02"]["average"] == pytest.approx(prior + CSC_NUDGE)
+
+
+def test_res_button_nudges_bucket_up_even_at_target_speed():
+  _, controller = make_controller()
+  prior = controller.learned_lat_accel(0.02)
+  controller.target = 20.0  # car tracking the target, so the gas-press condition would not fire
+
+  controller.handle_override(20.0, True, make_sm(), accel_button=True)
+  settle_override(controller)
+
+  assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT
+  assert controller.curvature_data["0.02"]["average"] > prior
+
+
+def test_brake_override_nudges_bucket_down():
+  _, controller = make_controller(driving_in_curve=True)
+  prior = controller.learned_lat_accel(0.02)
+
+  controller.handle_override(20.0, True, make_sm(brake=True))
+
+  assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT
+  assert controller.curvature_data["0.02"]["average"] < prior
+
+
+def test_calibrated_lateral_acceleration_param_is_written_on_flush():
+  planner, controller = make_controller(curvature_data={"0.02": {"average": 2.8, "count": 5000}})
+
+  assert "CalibratedLateralAcceleration" not in planner.params.values
+  controller.flush_data()
+
+  assert planner.params.values["CalibratedLateralAcceleration"] > DEFAULT_LATERAL_ACCELERATION
+  assert controller.lateral_acceleration == planner.params.values["CalibratedLateralAcceleration"]
+
+
+def test_stale_param_from_a_previous_build_is_republished_without_training():
+  # a stale value must not survive a restart just because this drive never trained
+  planner, controller = make_controller(curvature_data={"0.02": {"average": 2.8, "count": 5000}})
+  planner.params.values["CalibratedLateralAcceleration"] = 3.71
+
+  controller.log_data(0.0, make_sm())  # standstill: ineligible -> flush path
+
+  assert planner.params.values["CalibratedLateralAcceleration"] <= CSC_LAT_ACCEL_MAX

selfdrive/controls/tests/test_lead_follow_policy.py

@@ -60,6 +60,18 @@ def test_follow_policy_limits_small_post_lead_reversal():
   assert result.target - (-0.08) <= 0.18
 
 
+def test_follow_policy_deadbands_small_steady_sign_reversal():
+  result = run(lead(d_rel=37.2, v_lead=24.0, radar=True), v_ego=24.0, previous=0.24, raw=-0.24)
+
+  assert result.target == pytest.approx(0.0)
+
+
+def test_follow_policy_deadband_does_not_mask_closing_lead():
+  result = run(lead(d_rel=37.2, v_lead=22.5, a_lead=-1.0, radar=True), v_ego=24.0, previous=0.24, raw=-0.24)
+
+  assert result.target < 0.0
+
+
 def test_follow_policy_never_relaxes_material_braking():
   result = run(lead(d_rel=25.0, v_lead=18.0), v_ego=25.0, previous=0.30, raw=-1.2)
   assert result.target == pytest.approx(-1.2)

selfdrive/controls/tests/test_longcontrol.py

@@ -1235,6 +1235,37 @@ def test_toyota_sienna_target_filter_smooths_mild_high_speed_handoffs():
 
   assert -0.20 < filtered < 0.30
 
+def test_toyota_sienna_target_filter_smooths_nonurgent_low_speed_lead_braking():
+  CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
+  lead = SimpleNamespace(status=True, yRel=0.0, dRel=10.0, vLead=3.5, aLeadK=-0.4)
+
+  tuning.shape_toyota_sienna_accel_target(0.45, 5.0, False, leads=(lead,))
+  filtered = tuning.shape_toyota_sienna_accel_target(-1.0, 5.0, False, leads=(lead,))
+
+  assert -1.0 < filtered < 0.45
+
+
+def test_toyota_sienna_target_filter_keeps_urgent_low_speed_braking():
+  CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
+  lead = SimpleNamespace(status=True, yRel=0.0, dRel=5.0, vLead=0.0, aLeadK=-1.5)
+
+  filtered = tuning.shape_toyota_sienna_accel_target(-1.0, 3.0, False, leads=(lead,))
+
+  assert filtered == pytest.approx(-1.0)
+
+
+def test_toyota_sienna_target_filter_ramps_out_of_low_speed_braking_with_lead_present():
+  CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
+  lead = SimpleNamespace(status=True, yRel=0.0, dRel=6.5, vLead=1.5, aLeadK=-0.2)
+
+  tuning.shape_toyota_sienna_accel_target(-0.8, 1.5, False, leads=(lead,))
+  release = tuning.shape_toyota_sienna_accel_target(0.2, 1.5, False, leads=(lead,))
+
+  assert -0.8 < release < 0.0
+
 
 def test_toyota_sienna_target_filter_unwinds_braking_before_acceleration():
   CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -376,6 +376,17 @@ def test_model_lead_trajectory_falls_back_without_raw_lead_or_valid_shape():
   assert build_model_lead_trajectory(short_model_lead, raw_lead, 20.0) is None
 
 
+@pytest.mark.parametrize("d_rel,v_lead,a_lead", [
+  (42.0, 18.0, -0.6),
+  (8.0, 0.0, 0.0),
+])
+def test_model_lead_trajectory_falls_back_for_urgent_raw_lead(d_rel, v_lead, a_lead):
+  raw_lead = make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead, model_prob=0.99)
+  _, model_lead = make_model_lead()
+
+  assert build_model_lead_trajectory(model_lead, raw_lead, 20.0) is None
+
+
 def set_model_launch_trajectory(model, *, wait_time: float = 0.6, accel: float = 1.0):
   times = np.asarray(ModelConstants.T_IDXS, dtype=float)
   moving_time = np.maximum(times - wait_time, 0.0)

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -5,9 +5,10 @@ import pytest
 from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
 from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME
-from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController
+from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_GLOW_HOLD_TIME, CSC_GLOW_ON_DELTA
 from openpilot.starpilot.controls.lib.starpilot_vcruise import (
   FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME,
+  STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME,
   StarPilotVCruise,
   get_active_slc_control_target,
   get_lead_veto_distance,
@@ -46,6 +47,7 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False
     starpilot_following=SimpleNamespace(following_lead=False),
     tracking_lead=False,
     driving_in_curve=False,
+    lateral_acceleration=0.0,
     model_length=60.0,
     raw_model_stopped=raw_model_stopped,
     road_curvature=road_curvature,
@@ -77,12 +79,12 @@ def make_sm(*, standstill=True, min_steer_speed=0.0):
   }
 
 
-def update_vcruise(vcruise, sm, toggles, *, now, v_ego=0.0, controls_enabled=True):
+def update_vcruise(vcruise, sm, toggles, *, now, v_ego=0.0, v_cruise=20.0, controls_enabled=True):
   return vcruise.update(
     controls_enabled=controls_enabled,
     now=now,
     time_validated=True,
-    v_cruise=20.0,
+    v_cruise=v_cruise,
     v_ego=v_ego,
     sm=sm,
     starpilot_toggles=toggles,
@@ -130,30 +132,56 @@ def test_camry_tss2_gets_forward_force_stop_bias_only():
   assert get_force_stop_distance_bias("TOYOTA_RAV4_TSS2") == pytest.approx(0.0)
 
 
-def test_curve_speed_controller_holds_target_through_brief_detector_dropout():
+def test_curve_speed_controller_blinker_releases_the_cap_but_keeps_the_plan():
   planner, vcruise = make_vcruise()
   sm = make_sm(standstill=False)
   toggles = make_toggles()
   toggles.curve_speed_controller = True
 
-  def set_curve_target(_v_ego):
-    vcruise.csc.target_set = True
+  calls = []
+
+  def set_curve_target(_v_ego, _v_cruise):
+    calls.append(_v_ego)
     vcruise.csc.target = 14.0
 
   vcruise.csc.update_target = set_curve_target
-  planner.road_curvature_detected = True
   result = update_vcruise(vcruise, sm, toggles, now=10.0, v_ego=20.0)
   assert result == pytest.approx(14.0)
   assert vcruise.csc_controlling_speed
 
-  planner.road_curvature_detected = False
+  # the cap lifts so CSC can't fight the lane change, but the envelope keeps planning
+  # so the curve doesn't have to be re-discovered from the set speed afterwards
+  sm["carState"].leftBlinker = True
   result = update_vcruise(vcruise, sm, toggles, now=10.25, v_ego=20.0)
+  assert result == pytest.approx(20.0)
+  assert not vcruise.csc_controlling_speed
+  assert len(calls) == 2  # still planning, so nothing has to be rediscovered
+
+  # blinker off: the plan is already current, so the cap comes straight back
+  sm["carState"].leftBlinker = False
+  result = update_vcruise(vcruise, sm, toggles, now=10.5, v_ego=20.0)
   assert result == pytest.approx(14.0)
   assert vcruise.csc_controlling_speed
 
-  result = update_vcruise(vcruise, sm, toggles, now=10.8, v_ego=20.0)
-  assert result == pytest.approx(20.0)
+
+def test_curve_speed_controller_reseeds_after_a_real_dropout():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = 14.0
+
+  vcruise.csc.update_target = set_curve_target
+  update_vcruise(vcruise, sm, toggles, now=11.0, v_ego=20.0)
+  assert vcruise.csc_controlling_speed
+
+  # disengaging is a real dropout, not a momentary veto -- that still resets
+  sm["carControl"].longActive = False
+  update_vcruise(vcruise, sm, toggles, now=11.05, v_ego=20.0)
   assert not vcruise.csc_controlling_speed
+  assert vcruise.csc.seed_pending
 
 
 def test_curve_speed_controller_releases_immediately_when_disabled():
@@ -162,16 +190,13 @@ def test_curve_speed_controller_releases_immediately_when_disabled():
   toggles = make_toggles()
   toggles.curve_speed_controller = True
 
-  def set_curve_target(_v_ego):
-    vcruise.csc.target_set = True
+  def set_curve_target(_v_ego, _v_cruise):
     vcruise.csc.target = 14.0
 
   vcruise.csc.update_target = set_curve_target
-  planner.road_curvature_detected = True
   update_vcruise(vcruise, sm, toggles, now=20.0, v_ego=20.0)
   assert vcruise.csc_controlling_speed
 
-  planner.road_curvature_detected = False
   toggles.curve_speed_controller = False
   result = update_vcruise(vcruise, sm, toggles, now=20.1, v_ego=20.0)
   assert result == pytest.approx(20.0)
@@ -185,12 +210,10 @@ def test_curve_speed_controller_can_be_limited_to_driving_without_a_lead():
   toggles.curve_speed_controller = True
   toggles.csc_no_lead = True
 
-  def set_curve_target(_v_ego):
-    vcruise.csc.target_set = True
+  def set_curve_target(_v_ego, _v_cruise):
     vcruise.csc.target = 14.0
 
   vcruise.csc.update_target = set_curve_target
-  planner.road_curvature_detected = True
 
   result = update_vcruise(vcruise, sm, toggles, now=30.0, v_ego=20.0)
   assert result == pytest.approx(14.0)
@@ -208,10 +231,8 @@ def test_curve_speed_controller_stays_enabled_with_a_lead_by_default():
   toggles = make_toggles()
   toggles.curve_speed_controller = True
   planner.starpilot_following.following_lead = True
-  planner.road_curvature_detected = True
 
-  def set_curve_target(_v_ego):
-    vcruise.csc.target_set = True
+  def set_curve_target(_v_ego, _v_cruise):
     vcruise.csc.target = 14.0
 
   vcruise.csc.update_target = set_curve_target
@@ -261,39 +282,275 @@ def test_curve_speed_controller_persists_data_after_leaving_curve():
   assert any(key == "CurvatureData" for key, _ in planner.params.writes)
 
 
-def test_curve_speed_controller_ramps_toward_curve_speed_at_bounded_rate():
-  planner = SimpleNamespace(
-    params=FakeParams(),
-    road_curvature=0.004,
-    time_to_curve=2.0,
-    starpilot_weather=SimpleNamespace(weather_id=0, reduce_lateral_acceleration=0.0),
-  )
-  controller = CurveSpeedController(SimpleNamespace(starpilot_planner=planner))
-  controller.lateral_acceleration = 2.0
-  controller.target_set = True
-  controller.target = 30.0
+def test_csc_res_press_cancels_for_episode_and_rearms():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
 
-  controller.update_target(30.0)
+  curve_target = {"v": 14.0}
 
-  assert controller.target == pytest.approx(30.0 - CSC_MAX_DECEL_RATE * DT_MDL)
-  assert controller.target > (controller.lateral_acceleration / planner.road_curvature) ** 0.5
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = curve_target["v"]
+
+  vcruise.csc.update_target = set_curve_target
+  result = update_vcruise(vcruise, sm, toggles, now=60.0, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+  assert vcruise.csc_controlling_speed
+
+  sm["starpilotCarState"].accelPressed = True
+  result = update_vcruise(vcruise, sm, toggles, now=60.05, v_ego=20.0)
+  assert result == pytest.approx(20.0)
+  assert not vcruise.csc_controlling_speed
+  assert vcruise.csc_override
+
+  # latches for the rest of the episode, not just while pressed
+  sm["starpilotCarState"].accelPressed = False
+  result = update_vcruise(vcruise, sm, toggles, now=60.1, v_ego=20.0)
+  assert result == pytest.approx(20.0)
+  assert vcruise.csc_override
+
+  # curve ends -> re-arms
+  curve_target["v"] = 20.0
+  update_vcruise(vcruise, sm, toggles, now=60.15, v_ego=20.0)
+  assert not vcruise.csc_override
+
+  curve_target["v"] = 14.0
+  result = update_vcruise(vcruise, sm, toggles, now=60.2, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+  assert vcruise.csc_controlling_speed
 
 
-def test_curve_speed_controller_does_not_slow_for_curve_speed_above_ego():
-  planner = SimpleNamespace(
-    params=FakeParams(),
-    road_curvature=0.001,
-    time_to_curve=2.0,
-    starpilot_weather=SimpleNamespace(weather_id=0, reduce_lateral_acceleration=0.0),
-  )
-  controller = CurveSpeedController(SimpleNamespace(starpilot_planner=planner))
-  controller.lateral_acceleration = 2.0
-  controller.target_set = True
-  controller.target = 28.0
+def test_csc_res_press_does_not_latch_when_csc_was_not_active():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
 
-  controller.update_target(30.0)
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = 14.0
 
-  assert controller.target == pytest.approx(30.0)
+  vcruise.csc.update_target = set_curve_target
+  # press before CSC ever limited: suspends it while held, but must not latch a cancel
+  sm["starpilotCarState"].accelPressed = True
+  result = update_vcruise(vcruise, sm, toggles, now=70.0, v_ego=20.0)
+  assert result == pytest.approx(20.0)
+  assert not vcruise.csc_override
+
+  sm["starpilotCarState"].accelPressed = False
+  result = update_vcruise(vcruise, sm, toggles, now=70.05, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+  assert vcruise.csc_controlling_speed
+
+
+def test_csc_res_press_defers_to_slc_confirmation():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = 14.0
+
+  vcruise.csc.update_target = set_curve_target
+  update_vcruise(vcruise, sm, toggles, now=80.0, v_ego=20.0)
+  assert vcruise.csc_controlling_speed
+
+  # confirming a speed limit must not also cancel the curve slowdown
+  vcruise.slc.speed_limit_changed_timer = 1.0
+  vcruise.slc.unconfirmed_speed_limit = 25.0
+  sm["starpilotCarState"].accelPressed = True
+  update_vcruise(vcruise, sm, toggles, now=80.05, v_ego=20.0)
+  assert not vcruise.csc_override
+
+  sm["starpilotCarState"].accelPressed = False
+  result = update_vcruise(vcruise, sm, toggles, now=80.1, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+  assert vcruise.csc_controlling_speed
+
+
+def test_curve_speed_controller_glow_ignores_a_trivial_graze():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  # a long gentle bend where the envelope only shaves a little: the target hovers either
+  # side of the threshold for the whole curve, so a low bar strobes the glow
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = 20.0 - (CSC_GLOW_ON_DELTA / 2.0)
+
+  vcruise.csc.update_target = set_curve_target
+  result = update_vcruise(vcruise, sm, toggles, now=160.0, v_ego=20.0)
+
+  assert result < 20.0                      # the cap is still applied
+  assert not vcruise.csc_controlling_speed  # it just isn't worth announcing
+
+
+def test_curve_speed_controller_glow_holds_through_a_brief_release():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  curve_target = {"v": 14.0}
+
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = curve_target["v"]
+
+  vcruise.csc.update_target = set_curve_target
+  update_vcruise(vcruise, sm, toggles, now=130.0, v_ego=20.0)
+  assert vcruise.csc_controlling_speed
+
+  # one curve routinely lets go and re-engages; the glow must ride through it
+  curve_target["v"] = 20.0
+  now = 130.0
+  for _ in range(int((CSC_GLOW_HOLD_TIME - 0.2) / DT_MDL)):
+    now += DT_MDL
+    update_vcruise(vcruise, sm, toggles, now=now, v_ego=20.0)
+    assert vcruise.csc_controlling_speed
+
+  curve_target["v"] = 14.0
+  now += DT_MDL
+  update_vcruise(vcruise, sm, toggles, now=now, v_ego=20.0)
+  assert vcruise.csc_controlling_speed
+
+
+def test_curve_speed_controller_glow_clears_once_the_release_sticks():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  curve_target = {"v": 14.0}
+
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = curve_target["v"]
+
+  vcruise.csc.update_target = set_curve_target
+  update_vcruise(vcruise, sm, toggles, now=140.0, v_ego=20.0)
+  assert vcruise.csc_controlling_speed
+
+  curve_target["v"] = 20.0
+  now = 140.0
+  for _ in range(int(CSC_GLOW_HOLD_TIME / DT_MDL) + 1):
+    now += DT_MDL
+    update_vcruise(vcruise, sm, toggles, now=now, v_ego=20.0)
+  assert not vcruise.csc_controlling_speed
+
+
+def test_curve_speed_controller_keeps_the_cap_when_signalling_mid_curve():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = 14.0
+
+  vcruise.csc.update_target = set_curve_target
+  result = update_vcruise(vcruise, sm, toggles, now=150.0, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+
+  # a lane change taken inside a curve must not hand the speed back
+  planner.driving_in_curve = True
+  sm["carState"].leftBlinker = True
+  result = update_vcruise(vcruise, sm, toggles, now=150.05, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+  assert vcruise.csc_controlling_speed
+
+  # on a straight it still yields, so CSC can't fight the manoeuvre
+  planner.driving_in_curve = False
+  result = update_vcruise(vcruise, sm, toggles, now=150.1, v_ego=20.0)
+  assert result == pytest.approx(20.0)
+  assert not vcruise.csc_controlling_speed
+
+
+def test_curve_speed_controller_glow_lights_when_the_car_arrives_at_the_cap_from_below():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  # accelerating out of a slow zone into a curve: the target is never under v_ego, but it
+  # is still the only thing stopping the car from reaching the set speed
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = 22.0
+
+  vcruise.csc.update_target = set_curve_target
+
+  update_vcruise(vcruise, sm, toggles, now=120.0, v_ego=15.0, v_cruise=32.0)
+  assert not vcruise.csc_controlling_speed          # still climbing, CSC isn't holding it yet
+
+  result = update_vcruise(vcruise, sm, toggles, now=120.05, v_ego=22.0, v_cruise=32.0)
+  assert result == pytest.approx(22.0)
+  assert vcruise.csc_controlling_speed              # arrived at the cap, and it binds
+
+
+def test_curve_speed_controller_glow_stays_off_while_the_target_is_above_v_ego():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  # a highway sweeper trims the target well under the set speed but never under v_ego,
+  # so the car keeps accelerating and the driver feels nothing
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = 26.0
+
+  vcruise.csc.update_target = set_curve_target
+  result = update_vcruise(vcruise, sm, toggles, now=90.0, v_ego=20.0, v_cruise=30.0)
+
+  assert result == pytest.approx(26.0)
+  assert not vcruise.csc_controlling_speed
+
+
+def test_curve_speed_controller_glow_holds_through_the_recovery_ramp():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  curve_target = {"v": 14.0}
+
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = curve_target["v"]
+
+  vcruise.csc.update_target = set_curve_target
+  update_vcruise(vcruise, sm, toggles, now=100.0, v_ego=20.0)
+  assert vcruise.csc_controlling_speed
+
+  # past the apex the target climbs back above v_ego while the car is still cornering
+  curve_target["v"] = 18.0
+  update_vcruise(vcruise, sm, toggles, now=100.05, v_ego=15.0)
+  assert vcruise.csc_controlling_speed
+
+  # fully released, but the glow only clears once the release has stuck
+  curve_target["v"] = 20.0
+  now = 100.1
+  update_vcruise(vcruise, sm, toggles, now=now, v_ego=17.0)
+  assert vcruise.csc_controlling_speed
+
+  for _ in range(int(CSC_GLOW_HOLD_TIME / DT_MDL) + 1):
+    now += DT_MDL
+    update_vcruise(vcruise, sm, toggles, now=now, v_ego=17.0)
+  assert not vcruise.csc_controlling_speed
+
+
+def test_curve_speed_controller_hysteresis_keeps_glow_off_for_marginal_targets():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = 19.7
+
+  vcruise.csc.update_target = set_curve_target
+  result = update_vcruise(vcruise, sm, toggles, now=50.0, v_ego=20.0)
+
+  assert result == pytest.approx(19.7)
+  assert not vcruise.csc_controlling_speed
 
 
 def test_active_slc_control_target_applies_offset_and_cluster_diff():
@@ -492,6 +749,31 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens():
   assert vcruise.forcing_stop
 
 
+def test_force_stop_reanchors_when_model_reopens_path_without_stop_action():
+  planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
+  planner.model_length = 40.0
+  vcruise.tracked_model_length = 10.0
+  sm = make_sm(standstill=False)
+  sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
+
+  result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=1.5)
+
+  assert vcruise.tracked_model_length == pytest.approx(40.0)
+  assert result > 5.0
+
+
+def test_force_stop_does_not_reanchor_committed_model_stop():
+  planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
+  planner.model_length = 40.0
+  vcruise.tracked_model_length = 10.0
+  sm = make_sm(standstill=False)
+  sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=True))
+
+  update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=1.5)
+
+  assert vcruise.tracked_model_length < 10.0
+
+
 def test_force_stop_releases_after_cem_light_clears_while_moving():
   planner, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=True)
   sm = make_sm(standstill=False)
@@ -761,16 +1043,16 @@ def test_standstill_light_hold_expires_and_does_not_rearm_from_stopped_model():
   assert update_vcruise(vcruise, sm, toggles, now=0.0) == pytest.approx(0.0)
   assert vcruise.standstill_force_stop_reason == "light"
 
-  assert update_vcruise(vcruise, sm, toggles, now=4.9) == pytest.approx(0.0)
+  assert update_vcruise(vcruise, sm, toggles, now=STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME - 0.1) == pytest.approx(0.0)
   assert vcruise.forcing_stop
 
-  assert update_vcruise(vcruise, sm, toggles, now=5.1) == pytest.approx(20.0)
+  assert update_vcruise(vcruise, sm, toggles, now=STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME + 0.1) == pytest.approx(20.0)
   assert not vcruise.forcing_stop
   assert not vcruise.standstill_force_stop_hold
 
   # The red-light model remains stopped, but Force Stop must stay released so
   # Experimental Mode can own the red-to-green departure.
-  assert update_vcruise(vcruise, sm, toggles, now=5.2) == pytest.approx(20.0)
+  assert update_vcruise(vcruise, sm, toggles, now=STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME + 0.2) == pytest.approx(20.0)
   assert not vcruise.forcing_stop
 
 

selfdrive/locationd/models/generated/car.cpp

@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
  *                                                                            *
  *                         This file is part of 'ekf'                         *
  ******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_5328892797193692082) {
-   out_5328892797193692082[0] = delta_x[0] + nom_x[0];
-   out_5328892797193692082[1] = delta_x[1] + nom_x[1];
-   out_5328892797193692082[2] = delta_x[2] + nom_x[2];
-   out_5328892797193692082[3] = delta_x[3] + nom_x[3];
-   out_5328892797193692082[4] = delta_x[4] + nom_x[4];
-   out_5328892797193692082[5] = delta_x[5] + nom_x[5];
-   out_5328892797193692082[6] = delta_x[6] + nom_x[6];
-   out_5328892797193692082[7] = delta_x[7] + nom_x[7];
-   out_5328892797193692082[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_5232047554535600109) {
+   out_5232047554535600109[0] = delta_x[0] + nom_x[0];
+   out_5232047554535600109[1] = delta_x[1] + nom_x[1];
+   out_5232047554535600109[2] = delta_x[2] + nom_x[2];
+   out_5232047554535600109[3] = delta_x[3] + nom_x[3];
+   out_5232047554535600109[4] = delta_x[4] + nom_x[4];
+   out_5232047554535600109[5] = delta_x[5] + nom_x[5];
+   out_5232047554535600109[6] = delta_x[6] + nom_x[6];
+   out_5232047554535600109[7] = delta_x[7] + nom_x[7];
+   out_5232047554535600109[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_6664197118630309465) {
-   out_6664197118630309465[0] = -nom_x[0] + true_x[0];
-   out_6664197118630309465[1] = -nom_x[1] + true_x[1];
-   out_6664197118630309465[2] = -nom_x[2] + true_x[2];
-   out_6664197118630309465[3] = -nom_x[3] + true_x[3];
-   out_6664197118630309465[4] = -nom_x[4] + true_x[4];
-   out_6664197118630309465[5] = -nom_x[5] + true_x[5];
-   out_6664197118630309465[6] = -nom_x[6] + true_x[6];
-   out_6664197118630309465[7] = -nom_x[7] + true_x[7];
-   out_6664197118630309465[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_5926361318328117466) {
+   out_5926361318328117466[0] = -nom_x[0] + true_x[0];
+   out_5926361318328117466[1] = -nom_x[1] + true_x[1];
+   out_5926361318328117466[2] = -nom_x[2] + true_x[2];
+   out_5926361318328117466[3] = -nom_x[3] + true_x[3];
+   out_5926361318328117466[4] = -nom_x[4] + true_x[4];
+   out_5926361318328117466[5] = -nom_x[5] + true_x[5];
+   out_5926361318328117466[6] = -nom_x[6] + true_x[6];
+   out_5926361318328117466[7] = -nom_x[7] + true_x[7];
+   out_5926361318328117466[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_7049792320674666550) {
-   out_7049792320674666550[0] = 1.0;
-   out_7049792320674666550[1] = 0.0;
-   out_7049792320674666550[2] = 0.0;
-   out_7049792320674666550[3] = 0.0;
-   out_7049792320674666550[4] = 0.0;
-   out_7049792320674666550[5] = 0.0;
-   out_7049792320674666550[6] = 0.0;
-   out_7049792320674666550[7] = 0.0;
-   out_7049792320674666550[8] = 0.0;
-   out_7049792320674666550[9] = 0.0;
-   out_7049792320674666550[10] = 1.0;
-   out_7049792320674666550[11] = 0.0;
-   out_7049792320674666550[12] = 0.0;
-   out_7049792320674666550[13] = 0.0;
-   out_7049792320674666550[14] = 0.0;
-   out_7049792320674666550[15] = 0.0;
-   out_7049792320674666550[16] = 0.0;
-   out_7049792320674666550[17] = 0.0;
-   out_7049792320674666550[18] = 0.0;
-   out_7049792320674666550[19] = 0.0;
-   out_7049792320674666550[20] = 1.0;
-   out_7049792320674666550[21] = 0.0;
-   out_7049792320674666550[22] = 0.0;
-   out_7049792320674666550[23] = 0.0;
-   out_7049792320674666550[24] = 0.0;
-   out_7049792320674666550[25] = 0.0;
-   out_7049792320674666550[26] = 0.0;
-   out_7049792320674666550[27] = 0.0;
-   out_7049792320674666550[28] = 0.0;
-   out_7049792320674666550[29] = 0.0;
-   out_7049792320674666550[30] = 1.0;
-   out_7049792320674666550[31] = 0.0;
-   out_7049792320674666550[32] = 0.0;
-   out_7049792320674666550[33] = 0.0;
-   out_7049792320674666550[34] = 0.0;
-   out_7049792320674666550[35] = 0.0;
-   out_7049792320674666550[36] = 0.0;
-   out_7049792320674666550[37] = 0.0;
-   out_7049792320674666550[38] = 0.0;
-   out_7049792320674666550[39] = 0.0;
-   out_7049792320674666550[40] = 1.0;
-   out_7049792320674666550[41] = 0.0;
-   out_7049792320674666550[42] = 0.0;
-   out_7049792320674666550[43] = 0.0;
-   out_7049792320674666550[44] = 0.0;
-   out_7049792320674666550[45] = 0.0;
-   out_7049792320674666550[46] = 0.0;
-   out_7049792320674666550[47] = 0.0;
-   out_7049792320674666550[48] = 0.0;
-   out_7049792320674666550[49] = 0.0;
-   out_7049792320674666550[50] = 1.0;
-   out_7049792320674666550[51] = 0.0;
-   out_7049792320674666550[52] = 0.0;
-   out_7049792320674666550[53] = 0.0;
-   out_7049792320674666550[54] = 0.0;
-   out_7049792320674666550[55] = 0.0;
-   out_7049792320674666550[56] = 0.0;
-   out_7049792320674666550[57] = 0.0;
-   out_7049792320674666550[58] = 0.0;
-   out_7049792320674666550[59] = 0.0;
-   out_7049792320674666550[60] = 1.0;
-   out_7049792320674666550[61] = 0.0;
-   out_7049792320674666550[62] = 0.0;
-   out_7049792320674666550[63] = 0.0;
-   out_7049792320674666550[64] = 0.0;
-   out_7049792320674666550[65] = 0.0;
-   out_7049792320674666550[66] = 0.0;
-   out_7049792320674666550[67] = 0.0;
-   out_7049792320674666550[68] = 0.0;
-   out_7049792320674666550[69] = 0.0;
-   out_7049792320674666550[70] = 1.0;
-   out_7049792320674666550[71] = 0.0;
-   out_7049792320674666550[72] = 0.0;
-   out_7049792320674666550[73] = 0.0;
-   out_7049792320674666550[74] = 0.0;
-   out_7049792320674666550[75] = 0.0;
-   out_7049792320674666550[76] = 0.0;
-   out_7049792320674666550[77] = 0.0;
-   out_7049792320674666550[78] = 0.0;
-   out_7049792320674666550[79] = 0.0;
-   out_7049792320674666550[80] = 1.0;
+void H_mod_fun(double *state, double *out_8663045025294862621) {
+   out_8663045025294862621[0] = 1.0;
+   out_8663045025294862621[1] = 0.0;
+   out_8663045025294862621[2] = 0.0;
+   out_8663045025294862621[3] = 0.0;
+   out_8663045025294862621[4] = 0.0;
+   out_8663045025294862621[5] = 0.0;
+   out_8663045025294862621[6] = 0.0;
+   out_8663045025294862621[7] = 0.0;
+   out_8663045025294862621[8] = 0.0;
+   out_8663045025294862621[9] = 0.0;
+   out_8663045025294862621[10] = 1.0;
+   out_8663045025294862621[11] = 0.0;
+   out_8663045025294862621[12] = 0.0;
+   out_8663045025294862621[13] = 0.0;
+   out_8663045025294862621[14] = 0.0;
+   out_8663045025294862621[15] = 0.0;
+   out_8663045025294862621[16] = 0.0;
+   out_8663045025294862621[17] = 0.0;
+   out_8663045025294862621[18] = 0.0;
+   out_8663045025294862621[19] = 0.0;
+   out_8663045025294862621[20] = 1.0;
+   out_8663045025294862621[21] = 0.0;
+   out_8663045025294862621[22] = 0.0;
+   out_8663045025294862621[23] = 0.0;
+   out_8663045025294862621[24] = 0.0;
+   out_8663045025294862621[25] = 0.0;
+   out_8663045025294862621[26] = 0.0;
+   out_8663045025294862621[27] = 0.0;
+   out_8663045025294862621[28] = 0.0;
+   out_8663045025294862621[29] = 0.0;
+   out_8663045025294862621[30] = 1.0;
+   out_8663045025294862621[31] = 0.0;
+   out_8663045025294862621[32] = 0.0;
+   out_8663045025294862621[33] = 0.0;
+   out_8663045025294862621[34] = 0.0;
+   out_8663045025294862621[35] = 0.0;
+   out_8663045025294862621[36] = 0.0;
+   out_8663045025294862621[37] = 0.0;
+   out_8663045025294862621[38] = 0.0;
+   out_8663045025294862621[39] = 0.0;
+   out_8663045025294862621[40] = 1.0;
+   out_8663045025294862621[41] = 0.0;
+   out_8663045025294862621[42] = 0.0;
+   out_8663045025294862621[43] = 0.0;
+   out_8663045025294862621[44] = 0.0;
+   out_8663045025294862621[45] = 0.0;
+   out_8663045025294862621[46] = 0.0;
+   out_8663045025294862621[47] = 0.0;
+   out_8663045025294862621[48] = 0.0;
+   out_8663045025294862621[49] = 0.0;
+   out_8663045025294862621[50] = 1.0;
+   out_8663045025294862621[51] = 0.0;
+   out_8663045025294862621[52] = 0.0;
+   out_8663045025294862621[53] = 0.0;
+   out_8663045025294862621[54] = 0.0;
+   out_8663045025294862621[55] = 0.0;
+   out_8663045025294862621[56] = 0.0;
+   out_8663045025294862621[57] = 0.0;
+   out_8663045025294862621[58] = 0.0;
+   out_8663045025294862621[59] = 0.0;
+   out_8663045025294862621[60] = 1.0;
+   out_8663045025294862621[61] = 0.0;
+   out_8663045025294862621[62] = 0.0;
+   out_8663045025294862621[63] = 0.0;
+   out_8663045025294862621[64] = 0.0;
+   out_8663045025294862621[65] = 0.0;
+   out_8663045025294862621[66] = 0.0;
+   out_8663045025294862621[67] = 0.0;
+   out_8663045025294862621[68] = 0.0;
+   out_8663045025294862621[69] = 0.0;
+   out_8663045025294862621[70] = 1.0;
+   out_8663045025294862621[71] = 0.0;
+   out_8663045025294862621[72] = 0.0;
+   out_8663045025294862621[73] = 0.0;
+   out_8663045025294862621[74] = 0.0;
+   out_8663045025294862621[75] = 0.0;
+   out_8663045025294862621[76] = 0.0;
+   out_8663045025294862621[77] = 0.0;
+   out_8663045025294862621[78] = 0.0;
+   out_8663045025294862621[79] = 0.0;
+   out_8663045025294862621[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_8058289693715916474) {
-   out_8058289693715916474[0] = state[0];
-   out_8058289693715916474[1] = state[1];
-   out_8058289693715916474[2] = state[2];
-   out_8058289693715916474[3] = state[3];
-   out_8058289693715916474[4] = state[4];
-   out_8058289693715916474[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
-   out_8058289693715916474[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
-   out_8058289693715916474[7] = state[7];
-   out_8058289693715916474[8] = state[8];
+void f_fun(double *state, double dt, double *out_4712382669747920854) {
+   out_4712382669747920854[0] = state[0];
+   out_4712382669747920854[1] = state[1];
+   out_4712382669747920854[2] = state[2];
+   out_4712382669747920854[3] = state[3];
+   out_4712382669747920854[4] = state[4];
+   out_4712382669747920854[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
+   out_4712382669747920854[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
+   out_4712382669747920854[7] = state[7];
+   out_4712382669747920854[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_6834456908135559848) {
-   out_6834456908135559848[0] = 1;
-   out_6834456908135559848[1] = 0;
-   out_6834456908135559848[2] = 0;
-   out_6834456908135559848[3] = 0;
-   out_6834456908135559848[4] = 0;
-   out_6834456908135559848[5] = 0;
-   out_6834456908135559848[6] = 0;
-   out_6834456908135559848[7] = 0;
-   out_6834456908135559848[8] = 0;
-   out_6834456908135559848[9] = 0;
-   out_6834456908135559848[10] = 1;
-   out_6834456908135559848[11] = 0;
-   out_6834456908135559848[12] = 0;
-   out_6834456908135559848[13] = 0;
-   out_6834456908135559848[14] = 0;
-   out_6834456908135559848[15] = 0;
-   out_6834456908135559848[16] = 0;
-   out_6834456908135559848[17] = 0;
-   out_6834456908135559848[18] = 0;
-   out_6834456908135559848[19] = 0;
-   out_6834456908135559848[20] = 1;
-   out_6834456908135559848[21] = 0;
-   out_6834456908135559848[22] = 0;
-   out_6834456908135559848[23] = 0;
-   out_6834456908135559848[24] = 0;
-   out_6834456908135559848[25] = 0;
-   out_6834456908135559848[26] = 0;
-   out_6834456908135559848[27] = 0;
-   out_6834456908135559848[28] = 0;
-   out_6834456908135559848[29] = 0;
-   out_6834456908135559848[30] = 1;
-   out_6834456908135559848[31] = 0;
-   out_6834456908135559848[32] = 0;
-   out_6834456908135559848[33] = 0;
-   out_6834456908135559848[34] = 0;
-   out_6834456908135559848[35] = 0;
-   out_6834456908135559848[36] = 0;
-   out_6834456908135559848[37] = 0;
-   out_6834456908135559848[38] = 0;
-   out_6834456908135559848[39] = 0;
-   out_6834456908135559848[40] = 1;
-   out_6834456908135559848[41] = 0;
-   out_6834456908135559848[42] = 0;
-   out_6834456908135559848[43] = 0;
-   out_6834456908135559848[44] = 0;
-   out_6834456908135559848[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
-   out_6834456908135559848[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_6834456908135559848[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_6834456908135559848[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_6834456908135559848[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
-   out_6834456908135559848[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_6834456908135559848[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_6834456908135559848[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_6834456908135559848[53] = -9.8100000000000005*dt;
-   out_6834456908135559848[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
-   out_6834456908135559848[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_6834456908135559848[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_6834456908135559848[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_6834456908135559848[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
-   out_6834456908135559848[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_6834456908135559848[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
-   out_6834456908135559848[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_6834456908135559848[62] = 0;
-   out_6834456908135559848[63] = 0;
-   out_6834456908135559848[64] = 0;
-   out_6834456908135559848[65] = 0;
-   out_6834456908135559848[66] = 0;
-   out_6834456908135559848[67] = 0;
-   out_6834456908135559848[68] = 0;
-   out_6834456908135559848[69] = 0;
-   out_6834456908135559848[70] = 1;
-   out_6834456908135559848[71] = 0;
-   out_6834456908135559848[72] = 0;
-   out_6834456908135559848[73] = 0;
-   out_6834456908135559848[74] = 0;
-   out_6834456908135559848[75] = 0;
-   out_6834456908135559848[76] = 0;
-   out_6834456908135559848[77] = 0;
-   out_6834456908135559848[78] = 0;
-   out_6834456908135559848[79] = 0;
-   out_6834456908135559848[80] = 1;
+void F_fun(double *state, double dt, double *out_6132820248099245599) {
+   out_6132820248099245599[0] = 1;
+   out_6132820248099245599[1] = 0;
+   out_6132820248099245599[2] = 0;
+   out_6132820248099245599[3] = 0;
+   out_6132820248099245599[4] = 0;
+   out_6132820248099245599[5] = 0;
+   out_6132820248099245599[6] = 0;
+   out_6132820248099245599[7] = 0;
+   out_6132820248099245599[8] = 0;
+   out_6132820248099245599[9] = 0;
+   out_6132820248099245599[10] = 1;
+   out_6132820248099245599[11] = 0;
+   out_6132820248099245599[12] = 0;
+   out_6132820248099245599[13] = 0;
+   out_6132820248099245599[14] = 0;
+   out_6132820248099245599[15] = 0;
+   out_6132820248099245599[16] = 0;
+   out_6132820248099245599[17] = 0;
+   out_6132820248099245599[18] = 0;
+   out_6132820248099245599[19] = 0;
+   out_6132820248099245599[20] = 1;
+   out_6132820248099245599[21] = 0;
+   out_6132820248099245599[22] = 0;
+   out_6132820248099245599[23] = 0;
+   out_6132820248099245599[24] = 0;
+   out_6132820248099245599[25] = 0;
+   out_6132820248099245599[26] = 0;
+   out_6132820248099245599[27] = 0;
+   out_6132820248099245599[28] = 0;
+   out_6132820248099245599[29] = 0;
+   out_6132820248099245599[30] = 1;
+   out_6132820248099245599[31] = 0;
+   out_6132820248099245599[32] = 0;
+   out_6132820248099245599[33] = 0;
+   out_6132820248099245599[34] = 0;
+   out_6132820248099245599[35] = 0;
+   out_6132820248099245599[36] = 0;
+   out_6132820248099245599[37] = 0;
+   out_6132820248099245599[38] = 0;
+   out_6132820248099245599[39] = 0;
+   out_6132820248099245599[40] = 1;
+   out_6132820248099245599[41] = 0;
+   out_6132820248099245599[42] = 0;
+   out_6132820248099245599[43] = 0;
+   out_6132820248099245599[44] = 0;
+   out_6132820248099245599[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
+   out_6132820248099245599[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_6132820248099245599[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_6132820248099245599[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_6132820248099245599[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
+   out_6132820248099245599[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_6132820248099245599[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_6132820248099245599[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_6132820248099245599[53] = -9.8100000000000005*dt;
+   out_6132820248099245599[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
+   out_6132820248099245599[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_6132820248099245599[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_6132820248099245599[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_6132820248099245599[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
+   out_6132820248099245599[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_6132820248099245599[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
+   out_6132820248099245599[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_6132820248099245599[62] = 0;
+   out_6132820248099245599[63] = 0;
+   out_6132820248099245599[64] = 0;
+   out_6132820248099245599[65] = 0;
+   out_6132820248099245599[66] = 0;
+   out_6132820248099245599[67] = 0;
+   out_6132820248099245599[68] = 0;
+   out_6132820248099245599[69] = 0;
+   out_6132820248099245599[70] = 1;
+   out_6132820248099245599[71] = 0;
+   out_6132820248099245599[72] = 0;
+   out_6132820248099245599[73] = 0;
+   out_6132820248099245599[74] = 0;
+   out_6132820248099245599[75] = 0;
+   out_6132820248099245599[76] = 0;
+   out_6132820248099245599[77] = 0;
+   out_6132820248099245599[78] = 0;
+   out_6132820248099245599[79] = 0;
+   out_6132820248099245599[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_3817993858643920850) {
-   out_3817993858643920850[0] = state[6];
+void h_25(double *state, double *unused, double *out_8954572363719808224) {
+   out_8954572363719808224[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_2203763527122647829) {
-   out_2203763527122647829[0] = 0;
-   out_2203763527122647829[1] = 0;
-   out_2203763527122647829[2] = 0;
-   out_2203763527122647829[3] = 0;
-   out_2203763527122647829[4] = 0;
-   out_2203763527122647829[5] = 0;
-   out_2203763527122647829[6] = 1;
-   out_2203763527122647829[7] = 0;
-   out_2203763527122647829[8] = 0;
+void H_25(double *state, double *unused, double *out_6260718398425197318) {
+   out_6260718398425197318[0] = 0;
+   out_6260718398425197318[1] = 0;
+   out_6260718398425197318[2] = 0;
+   out_6260718398425197318[3] = 0;
+   out_6260718398425197318[4] = 0;
+   out_6260718398425197318[5] = 0;
+   out_6260718398425197318[6] = 1;
+   out_6260718398425197318[7] = 0;
+   out_6260718398425197318[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_589890583681194954) {
-   out_589890583681194954[0] = state[4];
-   out_589890583681194954[1] = state[5];
+void h_24(double *state, double *unused, double *out_573451915361062774) {
+   out_573451915361062774[0] = state[4];
+   out_573451915361062774[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_31113928117148263) {
-   out_31113928117148263[0] = 0;
-   out_31113928117148263[1] = 0;
-   out_31113928117148263[2] = 0;
-   out_31113928117148263[3] = 0;
-   out_31113928117148263[4] = 1;
-   out_31113928117148263[5] = 0;
-   out_31113928117148263[6] = 0;
-   out_31113928117148263[7] = 0;
-   out_31113928117148263[8] = 0;
-   out_31113928117148263[9] = 0;
-   out_31113928117148263[10] = 0;
-   out_31113928117148263[11] = 0;
-   out_31113928117148263[12] = 0;
-   out_31113928117148263[13] = 0;
-   out_31113928117148263[14] = 1;
-   out_31113928117148263[15] = 0;
-   out_31113928117148263[16] = 0;
-   out_31113928117148263[17] = 0;
+void H_24(double *state, double *unused, double *out_8433367997430696884) {
+   out_8433367997430696884[0] = 0;
+   out_8433367997430696884[1] = 0;
+   out_8433367997430696884[2] = 0;
+   out_8433367997430696884[3] = 0;
+   out_8433367997430696884[4] = 1;
+   out_8433367997430696884[5] = 0;
+   out_8433367997430696884[6] = 0;
+   out_8433367997430696884[7] = 0;
+   out_8433367997430696884[8] = 0;
+   out_8433367997430696884[9] = 0;
+   out_8433367997430696884[10] = 0;
+   out_8433367997430696884[11] = 0;
+   out_8433367997430696884[12] = 0;
+   out_8433367997430696884[13] = 0;
+   out_8433367997430696884[14] = 1;
+   out_8433367997430696884[15] = 0;
+   out_8433367997430696884[16] = 0;
+   out_8433367997430696884[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_855557586624953167) {
-   out_855557586624953167[0] = state[4];
+void h_30(double *state, double *unused, double *out_995511028726003400) {
+   out_995511028726003400[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_4722096485629896456) {
-   out_4722096485629896456[0] = 0;
-   out_4722096485629896456[1] = 0;
-   out_4722096485629896456[2] = 0;
-   out_4722096485629896456[3] = 0;
-   out_4722096485629896456[4] = 1;
-   out_4722096485629896456[5] = 0;
-   out_4722096485629896456[6] = 0;
-   out_4722096485629896456[7] = 0;
-   out_4722096485629896456[8] = 0;
+void H_30(double *state, double *unused, double *out_655971943066419437) {
+   out_655971943066419437[0] = 0;
+   out_655971943066419437[1] = 0;
+   out_655971943066419437[2] = 0;
+   out_655971943066419437[3] = 0;
+   out_655971943066419437[4] = 1;
+   out_655971943066419437[5] = 0;
+   out_655971943066419437[6] = 0;
+   out_655971943066419437[7] = 0;
+   out_655971943066419437[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_3459962907433650818) {
-   out_3459962907433650818[0] = state[7];
+void h_26(double *state, double *unused, double *out_8430654463154704213) {
+   out_8430654463154704213[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_1537739791751408395) {
-   out_1537739791751408395[0] = 0;
-   out_1537739791751408395[1] = 0;
-   out_1537739791751408395[2] = 0;
-   out_1537739791751408395[3] = 0;
-   out_1537739791751408395[4] = 0;
-   out_1537739791751408395[5] = 0;
-   out_1537739791751408395[6] = 0;
-   out_1537739791751408395[7] = 1;
-   out_1537739791751408395[8] = 0;
+void H_26(double *state, double *unused, double *out_8444522356410298074) {
+   out_8444522356410298074[0] = 0;
+   out_8444522356410298074[1] = 0;
+   out_8444522356410298074[2] = 0;
+   out_8444522356410298074[3] = 0;
+   out_8444522356410298074[4] = 0;
+   out_8444522356410298074[5] = 0;
+   out_8444522356410298074[6] = 0;
+   out_8444522356410298074[7] = 1;
+   out_8444522356410298074[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_2284525577089273540) {
-   out_2284525577089273540[0] = state[3];
+void h_27(double *state, double *unused, double *out_8548661857088080957) {
+   out_8548661857088080957[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_6945690556813839673) {
-   out_6945690556813839673[0] = 0;
-   out_6945690556813839673[1] = 0;
-   out_6945690556813839673[2] = 0;
-   out_6945690556813839673[3] = 1;
-   out_6945690556813839673[4] = 0;
-   out_6945690556813839673[5] = 0;
-   out_6945690556813839673[6] = 0;
-   out_6945690556813839673[7] = 0;
-   out_6945690556813839673[8] = 0;
+void H_27(double *state, double *unused, double *out_1518791368734005474) {
+   out_1518791368734005474[0] = 0;
+   out_1518791368734005474[1] = 0;
+   out_1518791368734005474[2] = 0;
+   out_1518791368734005474[3] = 1;
+   out_1518791368734005474[4] = 0;
+   out_1518791368734005474[5] = 0;
+   out_1518791368734005474[6] = 0;
+   out_1518791368734005474[7] = 0;
+   out_1518791368734005474[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_1458873471301430684) {
-   out_1458873471301430684[0] = state[1];
+void h_29(double *state, double *unused, double *out_589055427595770225) {
+   out_589055427595770225[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_5232327829944288640) {
-   out_5232327829944288640[0] = 0;
-   out_5232327829944288640[1] = 1;
-   out_5232327829944288640[2] = 0;
-   out_5232327829944288640[3] = 0;
-   out_5232327829944288640[4] = 0;
-   out_5232327829944288640[5] = 0;
-   out_5232327829944288640[6] = 0;
-   out_5232327829944288640[7] = 0;
-   out_5232327829944288640[8] = 0;
+void H_29(double *state, double *unused, double *out_3232154095603556507) {
+   out_3232154095603556507[0] = 0;
+   out_3232154095603556507[1] = 1;
+   out_3232154095603556507[2] = 0;
+   out_3232154095603556507[3] = 0;
+   out_3232154095603556507[4] = 0;
+   out_3232154095603556507[5] = 0;
+   out_3232154095603556507[6] = 0;
+   out_3232154095603556507[7] = 0;
+   out_3232154095603556507[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_7286198844750119185) {
-   out_7286198844750119185[0] = state[0];
+void h_28(double *state, double *unused, double *out_7867012193673341893) {
+   out_7867012193673341893[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_149928812874758066) {
-   out_149928812874758066[0] = 1;
-   out_149928812874758066[1] = 0;
-   out_149928812874758066[2] = 0;
-   out_149928812874758066[3] = 0;
-   out_149928812874758066[4] = 0;
-   out_149928812874758066[5] = 0;
-   out_149928812874758066[6] = 0;
-   out_149928812874758066[7] = 0;
-   out_149928812874758066[8] = 0;
+void H_28(double *state, double *unused, double *out_8314553112673087081) {
+   out_8314553112673087081[0] = 1;
+   out_8314553112673087081[1] = 0;
+   out_8314553112673087081[2] = 0;
+   out_8314553112673087081[3] = 0;
+   out_8314553112673087081[4] = 0;
+   out_8314553112673087081[5] = 0;
+   out_8314553112673087081[6] = 0;
+   out_8314553112673087081[7] = 0;
+   out_8314553112673087081[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_9220241001058989162) {
-   out_9220241001058989162[0] = state[8];
+void h_31(double *state, double *unused, double *out_2027016737343401600) {
+   out_2027016737343401600[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_2163947893984759871) {
-   out_2163947893984759871[0] = 0;
-   out_2163947893984759871[1] = 0;
-   out_2163947893984759871[2] = 0;
-   out_2163947893984759871[3] = 0;
-   out_2163947893984759871[4] = 0;
-   out_2163947893984759871[5] = 0;
-   out_2163947893984759871[6] = 0;
-   out_2163947893984759871[7] = 0;
-   out_2163947893984759871[8] = 1;
+void H_31(double *state, double *unused, double *out_6230072436548236890) {
+   out_6230072436548236890[0] = 0;
+   out_6230072436548236890[1] = 0;
+   out_6230072436548236890[2] = 0;
+   out_6230072436548236890[3] = 0;
+   out_6230072436548236890[4] = 0;
+   out_6230072436548236890[5] = 0;
+   out_6230072436548236890[6] = 0;
+   out_6230072436548236890[7] = 0;
+   out_6230072436548236890[8] = 1;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
   update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
 }
-void car_err_fun(double *nom_x, double *delta_x, double *out_5328892797193692082) {
-  err_fun(nom_x, delta_x, out_5328892797193692082);
+void car_err_fun(double *nom_x, double *delta_x, double *out_5232047554535600109) {
+  err_fun(nom_x, delta_x, out_5232047554535600109);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_6664197118630309465) {
-  inv_err_fun(nom_x, true_x, out_6664197118630309465);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_5926361318328117466) {
+  inv_err_fun(nom_x, true_x, out_5926361318328117466);
 }
-void car_H_mod_fun(double *state, double *out_7049792320674666550) {
-  H_mod_fun(state, out_7049792320674666550);
+void car_H_mod_fun(double *state, double *out_8663045025294862621) {
+  H_mod_fun(state, out_8663045025294862621);
 }
-void car_f_fun(double *state, double dt, double *out_8058289693715916474) {
-  f_fun(state,  dt, out_8058289693715916474);
+void car_f_fun(double *state, double dt, double *out_4712382669747920854) {
+  f_fun(state,  dt, out_4712382669747920854);
 }
-void car_F_fun(double *state, double dt, double *out_6834456908135559848) {
-  F_fun(state,  dt, out_6834456908135559848);
+void car_F_fun(double *state, double dt, double *out_6132820248099245599) {
+  F_fun(state,  dt, out_6132820248099245599);
 }
-void car_h_25(double *state, double *unused, double *out_3817993858643920850) {
-  h_25(state, unused, out_3817993858643920850);
+void car_h_25(double *state, double *unused, double *out_8954572363719808224) {
+  h_25(state, unused, out_8954572363719808224);
 }
-void car_H_25(double *state, double *unused, double *out_2203763527122647829) {
-  H_25(state, unused, out_2203763527122647829);
+void car_H_25(double *state, double *unused, double *out_6260718398425197318) {
+  H_25(state, unused, out_6260718398425197318);
 }
-void car_h_24(double *state, double *unused, double *out_589890583681194954) {
-  h_24(state, unused, out_589890583681194954);
+void car_h_24(double *state, double *unused, double *out_573451915361062774) {
+  h_24(state, unused, out_573451915361062774);
 }
-void car_H_24(double *state, double *unused, double *out_31113928117148263) {
-  H_24(state, unused, out_31113928117148263);
+void car_H_24(double *state, double *unused, double *out_8433367997430696884) {
+  H_24(state, unused, out_8433367997430696884);
 }
-void car_h_30(double *state, double *unused, double *out_855557586624953167) {
-  h_30(state, unused, out_855557586624953167);
+void car_h_30(double *state, double *unused, double *out_995511028726003400) {
+  h_30(state, unused, out_995511028726003400);
 }
-void car_H_30(double *state, double *unused, double *out_4722096485629896456) {
-  H_30(state, unused, out_4722096485629896456);
+void car_H_30(double *state, double *unused, double *out_655971943066419437) {
+  H_30(state, unused, out_655971943066419437);
 }
-void car_h_26(double *state, double *unused, double *out_3459962907433650818) {
-  h_26(state, unused, out_3459962907433650818);
+void car_h_26(double *state, double *unused, double *out_8430654463154704213) {
+  h_26(state, unused, out_8430654463154704213);
 }
-void car_H_26(double *state, double *unused, double *out_1537739791751408395) {
-  H_26(state, unused, out_1537739791751408395);
+void car_H_26(double *state, double *unused, double *out_8444522356410298074) {
+  H_26(state, unused, out_8444522356410298074);
 }
-void car_h_27(double *state, double *unused, double *out_2284525577089273540) {
-  h_27(state, unused, out_2284525577089273540);
+void car_h_27(double *state, double *unused, double *out_8548661857088080957) {
+  h_27(state, unused, out_8548661857088080957);
 }
-void car_H_27(double *state, double *unused, double *out_6945690556813839673) {
-  H_27(state, unused, out_6945690556813839673);
+void car_H_27(double *state, double *unused, double *out_1518791368734005474) {
+  H_27(state, unused, out_1518791368734005474);
 }
-void car_h_29(double *state, double *unused, double *out_1458873471301430684) {
-  h_29(state, unused, out_1458873471301430684);
+void car_h_29(double *state, double *unused, double *out_589055427595770225) {
+  h_29(state, unused, out_589055427595770225);
 }
-void car_H_29(double *state, double *unused, double *out_5232327829944288640) {
-  H_29(state, unused, out_5232327829944288640);
+void car_H_29(double *state, double *unused, double *out_3232154095603556507) {
+  H_29(state, unused, out_3232154095603556507);
 }
-void car_h_28(double *state, double *unused, double *out_7286198844750119185) {
-  h_28(state, unused, out_7286198844750119185);
+void car_h_28(double *state, double *unused, double *out_7867012193673341893) {
+  h_28(state, unused, out_7867012193673341893);
 }
-void car_H_28(double *state, double *unused, double *out_149928812874758066) {
-  H_28(state, unused, out_149928812874758066);
+void car_H_28(double *state, double *unused, double *out_8314553112673087081) {
+  H_28(state, unused, out_8314553112673087081);
 }
-void car_h_31(double *state, double *unused, double *out_9220241001058989162) {
-  h_31(state, unused, out_9220241001058989162);
+void car_h_31(double *state, double *unused, double *out_2027016737343401600) {
+  h_31(state, unused, out_2027016737343401600);
 }
-void car_H_31(double *state, double *unused, double *out_2163947893984759871) {
-  H_31(state, unused, out_2163947893984759871);
+void car_H_31(double *state, double *unused, double *out_6230072436548236890) {
+  H_31(state, unused, out_6230072436548236890);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
   predict(in_x, in_P, in_Q, dt);

selfdrive/locationd/models/generated/car.h

@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_5328892797193692082);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_6664197118630309465);
-void car_H_mod_fun(double *state, double *out_7049792320674666550);
-void car_f_fun(double *state, double dt, double *out_8058289693715916474);
-void car_F_fun(double *state, double dt, double *out_6834456908135559848);
-void car_h_25(double *state, double *unused, double *out_3817993858643920850);
-void car_H_25(double *state, double *unused, double *out_2203763527122647829);
-void car_h_24(double *state, double *unused, double *out_589890583681194954);
-void car_H_24(double *state, double *unused, double *out_31113928117148263);
-void car_h_30(double *state, double *unused, double *out_855557586624953167);
-void car_H_30(double *state, double *unused, double *out_4722096485629896456);
-void car_h_26(double *state, double *unused, double *out_3459962907433650818);
-void car_H_26(double *state, double *unused, double *out_1537739791751408395);
-void car_h_27(double *state, double *unused, double *out_2284525577089273540);
-void car_H_27(double *state, double *unused, double *out_6945690556813839673);
-void car_h_29(double *state, double *unused, double *out_1458873471301430684);
-void car_H_29(double *state, double *unused, double *out_5232327829944288640);
-void car_h_28(double *state, double *unused, double *out_7286198844750119185);
-void car_H_28(double *state, double *unused, double *out_149928812874758066);
-void car_h_31(double *state, double *unused, double *out_9220241001058989162);
-void car_H_31(double *state, double *unused, double *out_2163947893984759871);
+void car_err_fun(double *nom_x, double *delta_x, double *out_5232047554535600109);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_5926361318328117466);
+void car_H_mod_fun(double *state, double *out_8663045025294862621);
+void car_f_fun(double *state, double dt, double *out_4712382669747920854);
+void car_F_fun(double *state, double dt, double *out_6132820248099245599);
+void car_h_25(double *state, double *unused, double *out_8954572363719808224);
+void car_H_25(double *state, double *unused, double *out_6260718398425197318);
+void car_h_24(double *state, double *unused, double *out_573451915361062774);
+void car_H_24(double *state, double *unused, double *out_8433367997430696884);
+void car_h_30(double *state, double *unused, double *out_995511028726003400);
+void car_H_30(double *state, double *unused, double *out_655971943066419437);
+void car_h_26(double *state, double *unused, double *out_8430654463154704213);
+void car_H_26(double *state, double *unused, double *out_8444522356410298074);
+void car_h_27(double *state, double *unused, double *out_8548661857088080957);
+void car_H_27(double *state, double *unused, double *out_1518791368734005474);
+void car_h_29(double *state, double *unused, double *out_589055427595770225);
+void car_H_29(double *state, double *unused, double *out_3232154095603556507);
+void car_h_28(double *state, double *unused, double *out_7867012193673341893);
+void car_H_28(double *state, double *unused, double *out_8314553112673087081);
+void car_h_31(double *state, double *unused, double *out_2027016737343401600);
+void car_H_31(double *state, double *unused, double *out_6230072436548236890);
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
 void car_set_mass(double x);
 void car_set_rotational_inertia(double x);

selfdrive/locationd/models/generated/pose.h

@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void pose_err_fun(double *nom_x, double *delta_x, double *out_8334628911363265031);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6557509511038369963);
-void pose_H_mod_fun(double *state, double *out_2207458853057691014);
-void pose_f_fun(double *state, double dt, double *out_7635303320387631810);
-void pose_F_fun(double *state, double dt, double *out_8324840199262111301);
-void pose_h_4(double *state, double *unused, double *out_2118179578320955915);
-void pose_H_4(double *state, double *unused, double *out_7359977238096771749);
-void pose_h_10(double *state, double *unused, double *out_863109591849531819);
-void pose_H_10(double *state, double *unused, double *out_6053481103905099724);
-void pose_h_13(double *state, double *unused, double *out_8506504787708500859);
-void pose_H_13(double *state, double *unused, double *out_7874493010280447066);
-void pose_h_14(double *state, double *unused, double *out_6030639671543279411);
-void pose_H_14(double *state, double *unused, double *out_7123525979273295338);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_4038237010904669995);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2572217743337337909);
+void pose_H_mod_fun(double *state, double *out_362240871876916223);
+void pose_f_fun(double *state, double dt, double *out_844759421205290898);
+void pose_F_fun(double *state, double dt, double *out_3409614379123282877);
+void pose_h_4(double *state, double *unused, double *out_3761995773053554002);
+void pose_H_4(double *state, double *unused, double *out_4790277513162164512);
+void pose_h_10(double *state, double *unused, double *out_1433439433765456745);
+void pose_H_10(double *state, double *unused, double *out_2492873908575038793);
+void pose_h_13(double *state, double *unused, double *out_7701755112567992010);
+void pose_H_13(double *state, double *unused, double *out_8002551338494497313);
+void pose_h_14(double *state, double *unused, double *out_3517024732190759371);
+void pose_H_14(double *state, double *unused, double *out_1707489080866792216);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

selfdrive/modeld/modeld.py

@@ -933,10 +933,7 @@ def main(demo=False):
     mt2 = time.perf_counter()
     model_execution_time = mt2 - mt1
 
-    if model_output is not None and vipc_dropped_frames > 0:
-      cloudlog.error(f"suppressing model output after dropping {vipc_dropped_frames} frames")
-
-    if model_output is not None and vipc_dropped_frames == 0:
+    if model_output is not None:
       modelv2_send = messaging.new_message('modelV2')
       starpilot_modelv2_send = messaging.new_message('starpilotModelV2')
       drivingdata_send = messaging.new_message('drivingModelData')