Add PIPPreviewInvert parameter and update related functionality

This reverts commit 2db8d95905.

cereal/custom.capnp

@@ -265,6 +265,7 @@ struct StarPilotSelfdriveState @0xf416ec09499d9d19 {
   alertSize @3 :AlertSize;
   alertType @4 :Text;
   alertSound @5 :Car.CarControl.HUDControl.AudibleAlert;
+  vEgo @6 :Float32;
 
   enum AlertStatus {
     normal @0;

common/params_keys.h

@@ -345,6 +345,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"FordCurvatureBlendHigh", {PERSISTENT, FLOAT, "0.4", "0.4", 2}},
     {"FordCurvatureBlendLow", {PERSISTENT, FLOAT, "0.4", "0.4", 2}},
     {"FordCurvatureLaneChangeFactor", {PERSISTENT, FLOAT, "0.85", "0.85", 2}},
+    {"FordHandsFreeCluster", {PERSISTENT, BOOL, "0", "0", 2}},
     {"FordHumanTurnDetection", {PERSISTENT, BOOL, "1", "1", 2}},
     {"FordLateralMode", {PERSISTENT, INT, "1", "1", 2}},
     {"FLMActiveOverrides", {PERSISTENT, JSON, "{}", "{}", 2}},
@@ -516,6 +517,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"PIPPreviewMask", {PERSISTENT, JSON, "{\"width\":1928,\"height\":1208,\"center_left\":[315,548],\"center_right\":[1571,539],\"crop_size\":580}", "{\"width\":1928,\"height\":1208,\"center_left\":[315,548],\"center_right\":[1571,539],\"crop_size\":580}", 2}},
     {"PIPPreviewShowOnBlinker", {PERSISTENT, BOOL, "0", "0", 1}},
     {"PIPPreviewShowOnBSM", {PERSISTENT, BOOL, "0", "0", 1}},
+    {"PIPPreviewInvert", {PERSISTENT, BOOL, "0", "0", 1}},
     {"GalaxyPaired", {PERSISTENT, BOOL, "0", "0", 0}},
     {"GalaxyUploadPending", {PERSISTENT, BOOL, "0", "0", 0}},
     {"PreferredSchedule", {PERSISTENT, INT, "2", "0", 0}},
@@ -679,6 +681,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"TuningLevel", {PERSISTENT, INT, "0", "0", 0}},
     {"TuningLevelConfirmed", {PERSISTENT, BOOL, "0", "0", 0}},
     {"TurnDesires", {PERSISTENT, BOOL, "0", "0", 2}},
+    {"TurnSteeringLimitMuteSpeed", {PERSISTENT, INT, "0", "0", 0}},
     {"UnlockDoors", {PERSISTENT, BOOL, "1", "0", 0}},
     {"Updated", {PERSISTENT, STRING, "0", "0"}},
     {"UpdateSpeedLimits", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}},

launch_chffrplus.sh

@@ -147,101 +147,10 @@ function launch {
     while true; do sleep 1; done
   fi
 
-  function prebuilt_runtime_compatible {
-    python3 - <<'PY'
-import importlib
-import os
-from pathlib import Path
-import sys
-import time
-
-from openpilot.common.file_chunker import get_existing_chunks
-
-start = time.monotonic()
-last = start
-log_path = os.environ.get("SP_BOOT_TIMING_LOG")
-
-def emit(line):
-  print(line, flush=True)
-  if log_path:
-    try:
-      with open(log_path, "a") as f:
-        f.write(line + "\n")
-    except OSError:
-      pass
-
-def log_step(label):
-  global last
-  now = time.monotonic()
-  emit(f"SP_BOOT_TIMING prebuilt_compat {label} +{now - last:.3f}s total={now - start:.3f}s")
-  last = now
-
-mods = [
-  "openpilot.common.params_pyx",
-  "msgq.ipc_pyx",
-  "msgq.visionipc.visionipc_pyx",
-  "openpilot.common.transformations.transformations",
-  "openpilot.selfdrive.pandad.pandad_api_impl",
-  "openpilot.selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx",
-  "openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx",
-]
-
-for mod in mods:
-  try:
-    importlib.import_module(mod)
-  except Exception as e:
-    print(f"Prebuilt compatibility failure in {mod}: {e}", file=sys.stderr)
-    raise
-  log_step(f"import:{mod}")
-
-repo_root = Path.cwd().parents[1]
-required_model_artifacts = [
-  repo_root / "selfdrive/modeld/models/driving_tinygrad.pkl",
-  repo_root / "selfdrive/modeld/models/dmonitoring_model_metadata.pkl",
-  repo_root / "selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl",
-  repo_root / "selfdrive/modeld/models/dm_warp_1928x1208_tinygrad.pkl",
-  repo_root / "selfdrive/modeld/models/dm_warp_1344x760_tinygrad.pkl",
-]
-required_files = [
-  repo_root / "selfdrive/pandad/pandad_api_impl.so",
-  repo_root / "selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so",
-  repo_root / "selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/libacados_ocp_solver_lat.so",
-  repo_root / "selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so",
-  repo_root / "selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so",
-  repo_root / "opendbc_repo/opendbc/dbc/gm_global_a_powertrain_generated.dbc",
-]
-
-for path in required_model_artifacts:
-  try:
-    artifact_paths = [Path(p) for p in get_existing_chunks(path)]
-  except Exception as e:
-    raise FileNotFoundError(f"Missing prebuilt runtime artifact: {path}") from e
-  missing_chunks = [p for p in artifact_paths if not p.is_file()]
-  if missing_chunks:
-    missing = ", ".join(str(p) for p in missing_chunks)
-    raise FileNotFoundError(f"Missing prebuilt runtime artifact chunks for {path}: {missing}")
-log_step("required_model_artifacts")
-
-for path in required_files:
-  if not path.is_file():
-    raise FileNotFoundError(f"Missing prebuilt runtime artifact: {path}")
-log_step("required_files")
-PY
-  }
-
-  USE_PREBUILT=1
-  if [ -f /data/params/d/UsePrebuilt ]; then
-    USE_PREBUILT=$(tr -d '\n' < /data/params/d/UsePrebuilt)
-  fi
-
   sp_launch_timing "prebuilt_decision_done"
-  if [ "$USE_PREBUILT" = "1" ] && [ -f $DIR/prebuilt ] && ! prebuilt_runtime_compatible; then
-    echo "Prebuilt runtime artifacts are incompatible on this device; rebuilding locally."
-    USE_PREBUILT=0
-  fi
-  sp_launch_timing "prebuilt_compat_done"
-
-  if [ "$USE_PREBUILT" != "1" ] || [ ! -f $DIR/prebuilt ]; then
+  # Published trees carry this marker and must never compile on-device.
+  # Developers can remove it explicitly when working from a source tree.
+  if [ ! -f "$DIR/prebuilt" ]; then
     sp_launch_timing "build_start"
     ./build.py
     sp_launch_timing "build_done"

opendbc_repo/opendbc/car/ford/carcontroller.py

@@ -255,9 +255,15 @@ class CarController(CarControllerBase):
 
     if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
       show_distance_bars = self.frame - self.distance_bar_frame < 400
+      hands_free_cluster = bool(
+        self.ford_lateral is not None
+        and self.ford_lateral.mode != FordLateralMode.native
+        and self.ford_lateral.mode == self.ford_lateral_announced_mode
+        and self.ford_lateral.hands_free_cluster_enabled)
       can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
                                                  fcw_alert, CS.out.cruiseState.standstill, show_distance_bars,
-                                                 hud_control, CS.acc_tja_status_stock_values))
+                                                 hud_control, CS.acc_tja_status_stock_values,
+                                                 hands_free_cluster))
 
     self.main_on_last = main_on
     self.lkas_enabled_last = CC.latActive

opendbc_repo/opendbc/car/ford/fordcan.py

@@ -181,7 +181,7 @@ def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: fl
 
 
 def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool,
-                      show_distance_bars: bool, hud_control, stock_values: dict):
+                      show_distance_bars: bool, hud_control, stock_values: dict, hands_free_cluster: bool = False):
   """
   Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
   assist status.
@@ -197,6 +197,8 @@ def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw
       status = 3  # ActiveInterventionLeft
     elif hud_control.rightLaneDepart:
       status = 4  # ActiveInterventionRight
+    elif hands_free_cluster:
+      status = 7  # Hands-free assistance display
     else:
       status = 2  # Active
   elif main_on:

opendbc_repo/opendbc/car/ford/tests/test_ford.py

@@ -1,10 +1,13 @@
 import random
 from collections.abc import Iterable
+from types import SimpleNamespace
 
 from hypothesis import settings, given, strategies as st
 from parameterized import parameterized
 
 from opendbc.car import gen_empty_fingerprint
+from opendbc.can import CANPacker
+from opendbc.car.ford import fordcan
 from opendbc.car.structs import CarParams
 from opendbc.car.fw_versions import build_fw_dict
 from opendbc.car.ford.interface import CarInterface
@@ -172,3 +175,29 @@ def test_mach_e_longitudinal_toggle_controls_stock_acc_selection():
   assert enhanced.alphaLongitudinalAvailable
   assert enhanced.openpilotLongitudinalControl
   assert enhanced.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL
+
+
+def test_hands_free_cluster_status_is_opt_in():
+  packer = CANPacker("ford_lincoln_base_pt")
+  CAN = SimpleNamespace(main=0)
+  CP = SimpleNamespace(openpilotLongitudinalControl=False)
+  hud = SimpleNamespace(leftLaneDepart=False, rightLaneDepart=False)
+  stock_values = dict.fromkeys([
+    "HaDsply_No_Cs", "HaDsply_No_Cnt", "AccStopStat_D_Dsply", "AccTrgDist2_D_Dsply",
+    "AccStopRes_B_Dsply", "TjaWarn_D_Rq", "TjaMsgTxt_D_Dsply", "IaccLamp_D_Rq",
+    "AccMsgTxt_D2_Rq", "FcwDeny_B_Dsply", "FcwMemStat_B_Actl", "AccTGap_B_Dsply",
+    "CadsAlignIncplt_B_Actl", "AccFllwMde_B_Dsply", "CadsRadrBlck_B_Actl",
+    "CmbbPostEvnt_B_Dsply", "AccStopMde_B_Dsply", "FcwMemSens_D_Actl",
+    "FcwMsgTxt_D_Rq", "AccWarn_D_Dsply", "FcwVisblWarn_B_Rq", "FcwAudioWarn_B_Rq",
+    "AccTGap_D_Dsply", "AccMemEnbl_B_RqDrv", "FdaMem_B_Stat",
+  ], 0)
+
+  regular = fordcan.create_acc_ui_msg(
+    packer, CAN, CP, True, True, False, False, False, hud, stock_values)
+  hands_free = fordcan.create_acc_ui_msg(
+    packer, CAN, CP, True, True, False, False, False, hud, stock_values, True)
+  expected_regular = packer.make_can_msg("ACCDATA_3", 0, {"Tja_D_Stat": 2})
+  expected_hands_free = packer.make_can_msg("ACCDATA_3", 0, {"Tja_D_Stat": 7})
+
+  assert regular == expected_regular
+  assert hands_free == expected_hands_free

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

@@ -988,6 +988,45 @@ class TestHyundaiFingerprint:
     assert long_xceed.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.HYBRID_GAS
     assert long_xceed.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG
 
+  def test_g80_alpha_long_preserves_legacy_safety(self):
+    toggles = get_test_toggles()
+
+    stock_g80 = CarInterface.get_params(CAR.GENESIS_G80, gen_empty_fingerprint(), [], False, False, False, toggles)
+    assert CAR.GENESIS_G80 in LEGACY_LONGITUDINAL_CAR
+    assert stock_g80.alphaLongitudinalAvailable
+    assert not stock_g80.openpilotLongitudinalControl
+    assert stock_g80.pcmCruise
+    assert stock_g80.safetyConfigs[-1].safetyModel == CarParams.SafetyModel.hyundaiLegacy
+    assert not (stock_g80.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG)
+
+    long_g80 = CarInterface.get_params(CAR.GENESIS_G80, gen_empty_fingerprint(), [], True, False, False, toggles)
+    assert long_g80.alphaLongitudinalAvailable
+    assert long_g80.openpilotLongitudinalControl
+    assert not long_g80.pcmCruise
+    assert long_g80.safetyConfigs[-1].safetyModel == CarParams.SafetyModel.hyundaiLegacy
+    assert long_g80.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG
+
+  @pytest.mark.parametrize("ecu_disabled", (True, False))
+  def test_g80_alpha_long_disables_stock_scc(self, monkeypatch, ecu_disabled):
+    toggles = get_test_toggles()
+    CP = CarInterface.get_params(CAR.GENESIS_G80, gen_empty_fingerprint(), [], True, False, False, toggles)
+
+    called = {}
+
+    def fake_disable_ecu(*args, **kwargs):
+      called.update(kwargs)
+      return ecu_disabled
+
+    monkeypatch.setattr("opendbc.car.hyundai.interface.disable_ecu", fake_disable_ecu)
+    CarInterface.init(CP, None, None)
+
+    assert called["addr"] == 0x7d0
+    assert called["bus"] == 0
+    assert called["reset"] is False
+    assert CP.openpilotLongitudinalControl == ecu_disabled
+    assert CP.pcmCruise != ecu_disabled
+    assert bool(CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG) == ecu_disabled
+
   def test_xceed_phev_disable_failure_falls_back_to_stock_acc(self, monkeypatch):
     toggles = get_test_toggles()
     CP = CarInterface.get_params(CAR.KIA_XCEED_PHEV, gen_empty_fingerprint(), [], True, False, False, toggles)

opendbc_repo/opendbc/car/hyundai/values.py

@@ -1213,6 +1213,9 @@ NON_SCC_CAR = CAR.with_flags(HyundaiFlags.NON_SCC)
 #       HyundaiFlags.CANFD_RADAR_SCC | HyundaiFlags.CANFD_NO_RADAR_DISABLE | )
 UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.LEGACY) | CAR.with_flags(HyundaiFlags.UNSUPPORTED_LONGITUDINAL)
 
-LEGACY_LONGITUDINAL_CAR = {CAR.KIA_XCEED_PHEV}
+LEGACY_LONGITUDINAL_CAR = {
+  CAR.GENESIS_G80,
+  CAR.KIA_XCEED_PHEV,
+}
 
 DBC = CAR.create_dbc_map()

opendbc_repo/opendbc/car/nissan/interface.py

@@ -15,6 +15,7 @@ LEAF_ADAS_ECU_BUS = 0
 LEAF_ADAS_COMMAND_BUS = 1
 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'
+LEAF_2025_SV_PLUS_ALPHA_LONG_ENABLED = False
 
 LEAF_KWP_EXTENDED_REQUEST = b"\x10\xC0"
 LEAF_KWP_EXTENDED_RESPONSE = b"\x50\xC0"
@@ -26,7 +27,7 @@ LEAF_KWP_TAKEOVER_SESSIONS = (
 
 
 def is_leaf_2025_sv_plus_longitudinal(candidate, car_fw):
-  return candidate == CAR.NISSAN_LEAF and any(
+  return LEAF_2025_SV_PLUS_ALPHA_LONG_ENABLED and candidate == CAR.NISSAN_LEAF and any(
     fw.address == LEAF_ADAS_ECU_ADDR and bytes(fw.fwVersion) == LEAF_2025_SV_PLUS_CAMERA_FW
     for fw in car_fw
   )

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

@@ -4,6 +4,7 @@ import pytest
 
 from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs
 from opendbc.car.can_definitions import CanData
+from opendbc.car.nissan import interface as nissan_interface
 from opendbc.car.nissan.carstate import CarState
 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
@@ -18,6 +19,12 @@ SUPPORTED_LEAF_FW = [structs.CarParams.CarFw(
 )]
 
 
+@pytest.fixture
+def experimental_leaf_long(monkeypatch):
+  """Exercise the dormant implementation without making it available in production."""
+  monkeypatch.setattr(nissan_interface, "LEAF_2025_SV_PLUS_ALPHA_LONG_ENABLED", True)
+
+
 def run_controller(alpha_long, accel=0.0, long_active=True, long_state=structs.CarControl.Actuators.LongControlState.pid):
   CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, alpha_long, False, False, TEST_TOGGLES)
   FPCP = CarInterface.get_starpilot_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, CP, TEST_TOGGLES)
@@ -33,7 +40,28 @@ def run_controller(alpha_long, accel=0.0, long_active=True, long_state=structs.C
   return {msg[0]: msg for msg in can_sends}
 
 
-def test_leaf_2025_sv_plus_alpha_long_params():
+def test_leaf_2025_sv_plus_alpha_long_is_disabled(monkeypatch):
+  stock = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, False, False, False, None)
+  alpha_long = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
+
+  assert not stock.alphaLongitudinalAvailable
+  assert not stock.openpilotLongitudinalControl
+  assert stock.pcmCruise
+  assert not (stock.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL)
+
+  assert not alpha_long.alphaLongitudinalAvailable
+  assert not alpha_long.openpilotLongitudinalControl
+  assert alpha_long.pcmCruise
+  assert not alpha_long.autoResumeSng
+  assert not (alpha_long.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL)
+
+  disable_calls = []
+  monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: disable_calls.append((args, kwargs)))
+  CarInterface.init(alpha_long, None, None)
+  assert not disable_calls
+
+
+def test_dormant_leaf_2025_sv_plus_alpha_long_params(experimental_leaf_long):
   stock = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, False, False, False, None)
   alpha_long = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
 
@@ -79,7 +107,13 @@ def test_stock_controller_does_not_send_longitudinal_messages():
   assert not ({0x2B0, 0x1C3, 0x707} & can_sends.keys())
 
 
-def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive():
+def test_disabled_alpha_long_controller_does_not_send_longitudinal_messages():
+  can_sends = run_controller(True)
+
+  assert not ({0x2B0, 0x1C3, 0x707} & can_sends.keys())
+
+
+def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive(experimental_leaf_long):
   can_sends = run_controller(True)
 
   assert can_sends[0x2B0][1].hex() == "ff6090ac5b000e03"
@@ -89,13 +123,13 @@ def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive():
   assert can_sends[0x707][2] == 0
 
 
-def test_alpha_long_controller_clamps_to_panda_accel_limit():
+def test_alpha_long_controller_clamps_to_panda_accel_limit(experimental_leaf_long):
   can_sends = run_controller(True, accel=5.0)
 
   assert can_sends[0x2B0][1].hex() == "007f8fac5b000e0c"
 
 
-def test_alpha_long_controller_blends_friction_brake_below_regen_limit():
+def test_alpha_long_controller_blends_friction_brake_below_regen_limit(experimental_leaf_long):
   can_sends = run_controller(True, accel=-2.0)
 
   assert can_sends[0x2B0][1].hex() == "a827d5ac5b000e09"
@@ -104,7 +138,7 @@ def test_alpha_long_controller_blends_friction_brake_below_regen_limit():
   assert brake[5] & 0x84 == 0x84
 
 
-def test_alpha_long_controller_sends_inactive_commands_when_disengaged():
+def test_alpha_long_controller_sends_inactive_commands_when_disengaged(experimental_leaf_long):
   can_sends = run_controller(True, accel=1.0, long_active=False)
 
   assert can_sends[0x2B0][1].hex() == "dc53a2ac1b000e03"
@@ -113,7 +147,7 @@ def test_alpha_long_controller_sends_inactive_commands_when_disengaged():
 
 @pytest.mark.parametrize(("signal", "button_type"), [("SET_BUTTON", ButtonType.decelCruise),
                                                         ("RES_BUTTON", ButtonType.accelCruise)])
-def test_leaf_set_resume_release_enables_alpha_long(signal, button_type):
+def test_leaf_set_resume_release_enables_alpha_long(signal, button_type, experimental_leaf_long):
   CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, TEST_TOGGLES)
   FPCP = CarInterface.get_starpilot_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, CP, TEST_TOGGLES)
   CS = CarState(CP, FPCP)
@@ -130,7 +164,7 @@ def test_leaf_set_resume_release_enables_alpha_long(signal, button_type):
 
 
 @pytest.mark.parametrize("ecu_disabled", [False, True])
-def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled):
+def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled, experimental_leaf_long):
   CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
   calls = []
 
@@ -158,7 +192,7 @@ def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled):
   assert bool(CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL) is ecu_disabled
 
 
-def test_leaf_kwp_no_response_disable_can_confirm_ecu_silence(monkeypatch):
+def test_leaf_kwp_no_response_disable_can_confirm_ecu_silence(monkeypatch, experimental_leaf_long):
   CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
 
   monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: True)
@@ -171,7 +205,7 @@ def test_leaf_kwp_no_response_disable_can_confirm_ecu_silence(monkeypatch):
   assert CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL
 
 
-def test_leaf_positive_disable_response_without_command_silence_falls_back(monkeypatch):
+def test_leaf_positive_disable_response_without_command_silence_falls_back(monkeypatch, experimental_leaf_long):
   CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
   restore_calls = []
 

opendbc_repo/opendbc/car/tests/test_car_interfaces.py

@@ -165,13 +165,18 @@ class TestCarInterfaces:
       "Center_Stack_2": {"LKAS_Button": 1},
     }, is_ram=True)
 
-  def test_chrysler_wd_mod_enables_steer_to_zero(self):
+  @pytest.mark.parametrize("candidate", (
+    CHRYSLER_CAR.CHRYSLER_PACIFICA_2020,
+    CHRYSLER_CAR.JEEP_GRAND_CHEROKEE,
+    CHRYSLER_CAR.JEEP_GRAND_CHEROKEE_2019,
+  ))
+  def test_chrysler_steer_to_zero_module(self, candidate):
     fingerprint = {bus: {} for bus in range(8)}
     fingerprint[0][0x4FF] = 8
     toggles = get_test_starpilot_toggles()
 
     car_params = ChryslerCarInterface.get_params(
-      CHRYSLER_CAR.CHRYSLER_PACIFICA_2020,
+      candidate,
       fingerprint,
       [],
       alpha_long=False,
@@ -182,7 +187,7 @@ class TestCarInterfaces:
     assert car_params.minSteerSpeed > 0.
 
     fp_car_params = ChryslerCarInterface.get_starpilot_params(
-      CHRYSLER_CAR.CHRYSLER_PACIFICA_2020,
+      candidate,
       fingerprint,
       [],
       car_params,
@@ -206,11 +211,14 @@ class TestCarInterfaces:
       button_message="CRUISE_BUTTONS",
       auto_high_beam=0,
     )
+    controller.update(CC, CS, 0, toggles)
+    controller.update(CC, CS, 0, toggles)
     _, can_sends = controller.update(CC, CS, 0, toggles)
 
     lkas_parser = CANParser(CHRYSLER_DBC[car_params.carFingerprint][Bus.pt], [("LKAS_COMMAND", 50)], 0)
     lkas_parser.update([0, can_sends])
     assert lkas_parser.vl["LKAS_COMMAND"]["LKAS_CONTROL_BIT"] == 1
+    assert lkas_parser.vl["LKAS_COMMAND"]["STEERING_TORQUE"] != 0
 
   @pytest.mark.parametrize("candidate", (CHRYSLER_CAR.JEEP_GRAND_CHEROKEE, CHRYSLER_CAR.JEEP_GRAND_CHEROKEE_2019))
   def test_jeep_brake_hold_safety_capability_is_provisioned(self, candidate):

opendbc_repo/opendbc/safety/tests/common.py

@@ -1103,6 +1103,7 @@ class SafetyTest(SafetyTestBase):
                                               'TestHyundaiSafetyFCEVLong', 'TestHyundaiLongitudinalAolLkasOnEngageSafety',
                                               'TestHyundaiSafetyCanRefreshLong', 'TestHyundaiSafetyCanRefreshLongCameraSCC',
                                               'TestHyundaiCanCanfdBlendedLongitudinalSafety',
+                                              'TestHyundaiLegacyLongitudinalSafety',
                                               'TestHyundaiLegacyLongitudinalSafetyHEV'}):
               continue
             volkswagen_shared = ('TestVolkswagenMqb', 'TestVolkswagenMlb', 'TestVolkswagenMeb')
@@ -1156,6 +1157,7 @@ class SafetyTest(SafetyTestBase):
             if attr.startswith('TestHyundaiLongitudinal') or attr in ('TestHyundaiSafetyFCEVLong',
                                                                       'TestHyundaiLongitudinalAolLkasOnEngageSafety',
                                                                       'TestHyundaiCanCanfdBlendedLongitudinalSafety',
+                                                                      'TestHyundaiLegacyLongitudinalSafety',
                                                                       'TestHyundaiLegacyLongitudinalSafetyHEV'):
               # exceptions for common msgs across different Hyundai CAN platforms
               tx = list(filter(lambda m: m[0] not in [0x420, 0x50A, 0x389, 0x4A2], tx))

opendbc_repo/opendbc/safety/tests/test_hyundai.py

@@ -597,6 +597,14 @@ class TestHyundaiSafetyFCEVLong(TestHyundaiLongitudinalSafety, TestHyundaiSafety
     self.safety.init_tests()
 
 
+class TestHyundaiLegacyLongitudinalSafety(TestHyundaiLongitudinalSafety, TestHyundaiLegacySafety):
+  def setUp(self):
+    self.packer = CANPackerSafety("hyundai_kia_generic")
+    self.safety = libsafety_py.libsafety
+    self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiLegacy, HyundaiSafetyFlags.LONG)
+    self.safety.init_tests()
+
+
 class TestHyundaiLegacyLongitudinalSafetyHEV(TestHyundaiLongitudinalSafety, TestHyundaiLegacySafetyHEV):
   def setUp(self):
     self.packer = CANPackerSafety("hyundai_kia_generic")

panda/board/obj/gitversion.h

@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-284dbddd-DEBUG";
+const uint8_t gitversion[19] = "DEV-e28f72c6-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-284dbddd-DEBUG
+DEV-e28f72c6-DEBUG

selfdrive/controls/lib/latcontrol_torque.py

@@ -469,6 +469,8 @@ class LatControlTorque(LatControl):
         friction_threshold = CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD
         friction_scale = get_civic_bosch_modified_b_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
         friction_scale = 1.0 + ((friction_scale - 1.0) * civic_bosch_modified_a_center_taper)
+      if self.is_honda_accord:
+        ff *= get_honda_accord_ff_scale(setpoint)
       if flm_surface_active and self.flm_surface_profile_key and not ioniq_6_active:
         universal_flm_profile = self.flm_surface_profile_key == FLM_UNIVERSAL_PROFILE_KEY
         flm_full_surface_center_taper = get_flm_full_surface_center_taper_scale(self.flm_surface_profile_key, setpoint, CS.vEgo,
@@ -580,6 +582,8 @@ class LatControlTorque(LatControl):
         output_torque *= get_ram_1500_center_output_scale(setpoint, CS.vEgo)
         if output_torque * setpoint > 0.0:
           output_torque *= get_ram_1500_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
+        if setpoint * desired_lateral_jerk < 0.0:
+          output_torque *= get_ram_1500_unwind_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
       elif self.is_kona_non_scc:
         output_torque *= get_kona_non_scc_center_taper_scale(setpoint, CS.vEgo)
         rapid_reversal = setpoint * desired_lateral_jerk < 0.0

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -76,6 +76,9 @@ BOLT_CARS = BOLT_2022_2023_CARS + BOLT_2018_2021_CARS + BOLT_2017_CARS
 HONDA_ACCORD_STEER_RATIO_SCALE = 14.0 / 16.33
 HONDA_ACCORD_TORQUE_KP = 0.8
 HONDA_ACCORD_TORQUE_KI = 0.15
+HONDA_ACCORD_TURN_FF_REDUCTION_MAX = 0.10
+HONDA_ACCORD_TURN_FF_ONSET = 0.45
+HONDA_ACCORD_TURN_FF_WIDTH = 0.12
 VOLT_STANDARD_CARS = (
   GM_CAR.CHEVROLET_VOLT,
   GM_CAR.CHEVROLET_VOLT_2019,
@@ -433,7 +436,7 @@ GMC_YUKON_CC_UNWIND_FF_REDUCTION = 0.12
 
 SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.05
 SONATA_HYBRID_FF_REDUCTION_LEFT = 0.09
-SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.22
+SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.26
 SONATA_HYBRID_FF_ONSET = 0.18
 SONATA_HYBRID_FF_ONSET_WIDTH = 0.08
 SONATA_HYBRID_FF_CUTOFF = 1.35
@@ -444,7 +447,7 @@ SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.12
 SONATA_HYBRID_TURN_IN_BOOST_RIGHT = 0.02
 SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.18
 SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.10
-SONATA_HYBRID_CENTER_TAPER_MAX = 0.07
+SONATA_HYBRID_CENTER_TAPER_MAX = 0.10
 SONATA_HYBRID_CENTER_TAPER_LAT = 0.16
 SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH = 0.025
 SONATA_HYBRID_CENTER_TAPER_SPEED = 22.0
@@ -1178,6 +1181,13 @@ RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET = 5.5
 RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET_WIDTH = 1.5
 RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX = 16.0
 RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX_WIDTH = 2.0
+RAM_1500_UNWIND_OUTPUT_TAPER_MAX = 0.18
+RAM_1500_UNWIND_OUTPUT_SPEED_ONSET = 20.0
+RAM_1500_UNWIND_OUTPUT_SPEED_FULL = 29.0
+RAM_1500_UNWIND_OUTPUT_JERK_ONSET = 0.50
+RAM_1500_UNWIND_OUTPUT_JERK_FULL = 1.80
+RAM_1500_UNWIND_OUTPUT_LAT_ONSET = 0.65
+RAM_1500_UNWIND_OUTPUT_LAT_WIDTH = 0.30
 
 # The Kona route is exceptionally accurate below highway speed, but Pop V2
 # reverses the requested lateral acceleration roughly once per second at
@@ -1787,6 +1797,23 @@ def get_ram_1500_transition_output_scale(desired_lateral_accel: float, desired_l
   return 1.0 - (RAM_1500_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight)
 
 
+def get_ram_1500_unwind_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float,
+                                     v_ego: float) -> float:
+  """Soften only rapid high-speed unwind reversals on the RAM 1500."""
+  if desired_lateral_accel * desired_lateral_jerk >= 0.0:
+    return 1.0
+
+  speed_weight = float(np.interp(v_ego,
+                                 [RAM_1500_UNWIND_OUTPUT_SPEED_ONSET, RAM_1500_UNWIND_OUTPUT_SPEED_FULL],
+                                 [0.0, 1.0]))
+  jerk_weight = float(np.interp(abs(desired_lateral_jerk),
+                                [RAM_1500_UNWIND_OUTPUT_JERK_ONSET, RAM_1500_UNWIND_OUTPUT_JERK_FULL],
+                                [0.0, 1.0]))
+  curve_weight = _sigmoid((abs(desired_lateral_accel) - RAM_1500_UNWIND_OUTPUT_LAT_ONSET) /
+                           RAM_1500_UNWIND_OUTPUT_LAT_WIDTH)
+  return 1.0 - (RAM_1500_UNWIND_OUTPUT_TAPER_MAX * speed_weight * jerk_weight * curve_weight)
+
+
 def get_ram_1500_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
   """Damp only center corrections at low/mid speed, not turn authority."""
   center_weight = _sigmoid((RAM_1500_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
@@ -2005,6 +2032,13 @@ def get_honda_accord_steer_ratio_scale(_v_ego: float) -> float:
   return HONDA_ACCORD_STEER_RATIO_SCALE
 
 
+def get_honda_accord_ff_scale(desired_lateral_accel: float) -> float:
+  """Taper only sharp-turn feedforward where the Accord carries excess curvature."""
+  turn_weight = _sigmoid((abs(desired_lateral_accel) - HONDA_ACCORD_TURN_FF_ONSET) /
+                         HONDA_ACCORD_TURN_FF_WIDTH)
+  return 1.0 - (HONDA_ACCORD_TURN_FF_REDUCTION_MAX * turn_weight)
+
+
 def get_bolt_2017_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
   center_window = _bolt_2017_sigmoid((BOLT_2017_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2017_CENTER_TAPER_WIDTH)
   return 1.0 - (BOLT_2017_CENTER_TAPER_GAIN * _bolt_2017_high_speed_factor(v_ego) * center_window)

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -923,7 +923,8 @@ class LongitudinalMpc:
   def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow,
              personality=log.LongitudinalPersonality.standard, tracking_lead=True,
              optional_far_lead_comfort=True, smooth_duplicate_vision=False,
-             stop_x=None, silverado_early_follow=False, modelV2=None):
+             stop_x=None, silverado_early_follow=False, modelV2=None,
+             lead_obstacle_bias=(0.0, 0.0)):
     v_ego = self.x0[1]
     lead_one = radarstate.leadOne
     lead_two = radarstate.leadTwo
@@ -943,6 +944,8 @@ class LongitudinalMpc:
     # and then treat that as a stopped car/obstacle at this new distance.
     lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
     lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+    lead_0_obstacle -= float(lead_obstacle_bias[0])
+    lead_1_obstacle -= float(lead_obstacle_bias[1])
 
     self.params[:,0] = ACCEL_MIN
     self.params[:,1] = max(0.0, self.max_a)

selfdrive/controls/lib/longitudinal_planner.py

@@ -21,6 +21,8 @@ from openpilot.selfdrive.controls.lib.lead_follow_policy import is_nonurgent_dup
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   get_far_follow_output_slew_rates,
   get_follow_prebrake_min_headway,
+  get_honda_accord_lead_departure_tune,
+  get_toyota_rav4_tss2_lead_departure_tune,
   get_force_stop_distance_bias,
   get_force_stop_handoff_distance,
   allow_radar_standstill_gap_settle,
@@ -30,6 +32,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   is_toyota_rav4_tss2_radar_follow_lead,
   get_toyota_sienna_post_departure_restop_cap,
   get_untracked_slow_lead_decel_scale,
+  get_toyota_prius_stopped_lead_obstacle_bias,
 )
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
@@ -1505,9 +1508,14 @@ class LongitudinalPlanner:
                                max(LEAD_DEPART_ACCEL_HOLD_FULL_GAP - LEAD_DEPART_ACCEL_HOLD_MIN_GAP, 0.1), 0.0, 1.0))
     lead_factor = float(np.clip((lead_speed - LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED) /
                                 max(LEAD_DEPART_ACCEL_HOLD_FULL_LEAD_SPEED - LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED, 0.1), 0.0, 1.0))
-    accel_cap = LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL + (LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL - LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL) * np.clip(
+    departure_tune = get_honda_accord_lead_departure_tune(self.CP)
+    if departure_tune is None:
+      departure_tune = get_toyota_rav4_tss2_lead_departure_tune(self.CP)
+    max_accel = LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL if departure_tune is None else departure_tune[0]
+    assist = LEAD_DEPART_ACCEL_ASSIST if departure_tune is None else departure_tune[1]
+    accel_cap = LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL + (max_accel - LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL) * np.clip(
       0.55 * lead_factor + 0.45 * gap_factor, 0.0, 1.0)
-    assisted_model_accel = float(model_desired_accel) + LEAD_DEPART_ACCEL_ASSIST
+    assisted_model_accel = float(model_desired_accel) + assist
     return min(accel_cap, max(assisted_model_accel, LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL))
 
   def get_reusable_lead_depart_accel_floor(self, lead, v_ego, t_follow):
@@ -2201,6 +2209,19 @@ class LongitudinalPlanner:
         get_force_stop_distance_bias(self.CP.carFingerprint)
       )
 
+    prius_lead_obstacle_bias = (0.0, 0.0)
+    if (
+      self.mode == 'acc' and
+      not bool(getattr(sm['modelV2'].action, 'shouldStop', False)) and
+      not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and
+      not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and
+      not bool(getattr(sm['carState'], 'standstill', False))
+    ):
+      prius_lead_obstacle_bias = (
+        get_toyota_prius_stopped_lead_obstacle_bias(self.CP, self.lead_one, scene_v_ego),
+        get_toyota_prius_stopped_lead_obstacle_bias(self.CP, self.lead_two, scene_v_ego),
+      )
+
     self.mpc.update(sm['radarState'], v_cruise, x, v, a, j,
                     sm['starpilotPlan'].dangerFactor, effective_t_follow,
                     personality=personality, tracking_lead=lead_control_active,
@@ -2208,7 +2229,8 @@ class LongitudinalPlanner:
                     smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass,
                     stop_x=force_stop_x,
                     silverado_early_follow=early_truck_follow,
-                    modelV2=sm['modelV2'])
+                    modelV2=sm['modelV2'],
+                    lead_obstacle_bias=prius_lead_obstacle_bias)
 
     self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)

selfdrive/controls/lib/longitudinal_vehicle_tunes.py

@@ -4,6 +4,8 @@ import numpy as np
 HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE = 3.0
 HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE = 2.0
 HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE = 1.35
+HONDA_ACCORD_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.85
+HONDA_ACCORD_LEAD_DEPART_ACCEL_ASSIST = 0.25
 HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25
 GM_SILVERADO_EARLY_FOLLOW_MIN_EGO_SPEED = 18.0
 GM_SILVERADO_EARLY_FOLLOW_MAX_DISTANCE = 130.0
@@ -36,6 +38,15 @@ TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_OFFSET = 32.0
 TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_LATERAL_OFFSET = 1.75
 TOYOTA_RAV4_TSS2_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5
 TOYOTA_RAV4_TSS2_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75
+TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.70
+TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_ASSIST = 0.20
+TOYOTA_PRIUS_STOPPED_LEAD_OBSTACLE_BIAS_M = 1.5
+TOYOTA_PRIUS_STOPPED_LEAD_MAX_EGO_SPEED = 22.0
+TOYOTA_PRIUS_STOPPED_LEAD_MAX_SPEED = 1.0
+TOYOTA_PRIUS_STOPPED_LEAD_MIN_CLOSING_SPEED = 0.15
+TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE = 80.0
+TOYOTA_PRIUS_STOPPED_LEAD_RAMP_DISTANCE = 10.0
+TOYOTA_PRIUS_STOPPED_LEAD_MAX_LATERAL_OFFSET = 1.75
 TOYOTA_CAMRY_TSS2_FORCE_STOP_HANDOFF_M = 4.5
 # The Camry's force-stop path otherwise consumes the model endpoint before the
 # normal MPC stop-distance margin can be applied. Keep it within the forward
@@ -44,6 +55,35 @@ TOYOTA_CAMRY_TSS2_FORCE_STOP_DISTANCE_BIAS_M = 6.0
 DEFAULT_FORCE_STOP_HANDOFF_M = 6.0
 
 
+def get_toyota_prius_stopped_lead_obstacle_bias(CP, lead, v_ego):
+  """Move the ordinary Prius stopped-lead target back without touching stop targets."""
+  if (
+    getattr(CP, "brand", "") != "toyota" or
+    str(getattr(CP, "carFingerprint", "")) != "TOYOTA_PRIUS" or
+    lead is None or not bool(getattr(lead, "status", False)) or
+    float(v_ego) <= 0.0 or float(v_ego) > TOYOTA_PRIUS_STOPPED_LEAD_MAX_EGO_SPEED or
+    float(getattr(lead, "vLead", 0.0)) > TOYOTA_PRIUS_STOPPED_LEAD_MAX_SPEED or
+    abs(float(getattr(lead, "yRel", 0.0))) > TOYOTA_PRIUS_STOPPED_LEAD_MAX_LATERAL_OFFSET
+  ):
+    return 0.0
+
+  distance = float(getattr(lead, "dRel", float("inf")))
+  closing_speed = float(v_ego) - float(getattr(lead, "vLead", 0.0))
+  if (
+    distance <= 0.0 or distance > TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE or
+    closing_speed < TOYOTA_PRIUS_STOPPED_LEAD_MIN_CLOSING_SPEED
+  ):
+    return 0.0
+
+  strength = np.clip(
+    (TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE - distance) /
+    (TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE - TOYOTA_PRIUS_STOPPED_LEAD_RAMP_DISTANCE),
+    0.0, 1.0,
+  )
+  bias = TOYOTA_PRIUS_STOPPED_LEAD_OBSTACLE_BIAS_M * strength
+  return float(min(bias, max(distance - 0.5, 0.0)))
+
+
 def is_toyota_rav4_tss2_post_departure_tune(CP):
   """Identify RAV4 TSS2 variants that need normal catch-up caps after departure."""
   return (
@@ -52,6 +92,15 @@ def is_toyota_rav4_tss2_post_departure_tune(CP):
   )
 
 
+def get_toyota_rav4_tss2_lead_departure_tune(CP):
+  if is_toyota_rav4_tss2_post_departure_tune(CP):
+    return (
+      TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL,
+      TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_ASSIST,
+    )
+  return None
+
+
 def get_toyota_rav4_tss2_early_lead_cap(CP, lead, v_ego, accel_min):
   """Start a mild RAV4 coast/brake response before a hard lead approach."""
   if (
@@ -155,6 +204,15 @@ def get_lead_follow_jerk_scale(CP):
   return 1.0
 
 
+def get_honda_accord_lead_departure_tune(CP):
+  if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD":
+    return (
+      HONDA_ACCORD_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL,
+      HONDA_ACCORD_LEAD_DEPART_ACCEL_ASSIST,
+    )
+  return None
+
+
 def is_gm_silverado_early_follow_lead(CP, lead, v_ego):
   """Admit a credible centered vision lead before it becomes a close lead."""
   if (

selfdrive/controls/tests/test_conditional_chill_mode.py

@@ -29,11 +29,14 @@ class FakeDetector:
     self.slow_lead_detected = False
     self.stop_light_detected = False
     self.stop_light_model_detected = False
+    self.toggle_objects = []
 
-  def curve_detection(self, *_args, **_kwargs):
+  def curve_detection(self, _v_ego, toggles):
+    self.toggle_objects.append(toggles)
     return None
 
-  def slow_lead(self, *_args, **_kwargs):
+  def slow_lead(self, toggles, _v_ego):
+    self.toggle_objects.append(toggles)
     return None
 
   def stop_sign_and_light(self, *_args, **_kwargs):
@@ -114,6 +117,23 @@ def test_ccm_stays_experimental_when_no_chill_condition_matches(monkeypatch):
   assert ccm.status_value == CCStatus["OFF"]
 
 
+def test_ccm_reuses_detector_toggles(monkeypatch):
+  _planner, detector, ccm = make_ccm()
+  sm = make_sm()
+  toggles = make_toggles()
+  monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0)
+
+  ccm.update(20 * CV.MPH_TO_MS, 21 * CV.MPH_TO_MS, sm, toggles)
+  first_curve_toggles, first_slow_lead_toggles = detector.toggle_objects
+  detector.toggle_objects.clear()
+  ccm.update(20 * CV.MPH_TO_MS, 21 * CV.MPH_TO_MS, sm, toggles)
+  second_curve_toggles, second_slow_lead_toggles = detector.toggle_objects
+
+  assert first_curve_toggles is first_slow_lead_toggles
+  assert second_curve_toggles is second_slow_lead_toggles
+  assert first_curve_toggles is second_curve_toggles
+
+
 def test_ccm_enters_chill_for_open_road_speed_recovery(monkeypatch):
   planner, _detector, ccm = make_ccm()
   sm = make_sm()

selfdrive/controls/tests/test_latcontrol.py

@@ -43,6 +43,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
   get_gmc_yukon_cc_ff_scale,
   get_ram_1500_center_output_scale,
   get_ram_1500_transition_output_scale,
+  get_ram_1500_unwind_output_scale,
   get_ram_1500_ff_scale,
   get_rav4_tss2_pid_output,
   get_subaru_impreza_pid_output_scale,
@@ -86,6 +87,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   get_genesis_gv70_friction_threshold,
   get_genesis_gv70_high_speed_error_scale,
   get_genesis_gv70_unwind_ff_scale,
+  get_honda_accord_ff_scale,
   get_elantra_non_scc_ff_scale,
   get_honda_accord_steer_ratio_scale,
   get_palisade_ff_scale,
@@ -1079,6 +1081,18 @@ class TestLatControl:
     assert crawl > center
     assert center > 0.85
 
+  def test_ram_1500_unwind_output_taper_is_high_speed_and_phase_gated(self):
+    turn_in = get_ram_1500_unwind_output_scale(1.2, 1.1, 25.0)
+    low_speed = get_ram_1500_unwind_output_scale(1.2, -1.1, 15.0)
+    high_speed = get_ram_1500_unwind_output_scale(1.2, -1.1, 25.0)
+    sharp_reversal = get_ram_1500_unwind_output_scale(2.4, -2.0, 29.0)
+
+    assert turn_in == pytest.approx(1.0)
+    assert low_speed == pytest.approx(1.0)
+    assert 0.95 < high_speed < 1.0
+    assert sharp_reversal < high_speed
+    assert sharp_reversal > 0.80
+
   def test_ram_1500_phase_feedforward_curve(self):
     assert get_ram_1500_ff_scale(0.0, 1.0, 15.0) == pytest.approx(1.0)
     assert get_ram_1500_ff_scale(1.2, 1.1, 17.0) > 1.0
@@ -1761,6 +1775,11 @@ class TestLatControl:
     assert get_honda_accord_steer_ratio_scale(0.0) == pytest.approx(expected_scale)
     assert get_honda_accord_steer_ratio_scale(20.0) == pytest.approx(expected_scale)
 
+  def test_honda_accord_turn_feedforward_taper(self):
+    assert get_honda_accord_ff_scale(0.0) > get_honda_accord_ff_scale(0.8)
+    assert get_honda_accord_ff_scale(-0.8) == pytest.approx(get_honda_accord_ff_scale(0.8))
+    assert get_honda_accord_ff_scale(0.0) == pytest.approx(1.0, abs=0.01)
+
   def test_subaru_impreza_pid_output_scale_preserves_small_errors(self):
     assert get_subaru_impreza_pid_output_scale(0.0) == 1.0
     assert get_subaru_impreza_pid_output_scale(0.75) == 1.0

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -27,6 +27,9 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   allow_radar_standstill_gap_settle,
   get_far_follow_output_slew_rates,
   get_follow_prebrake_min_headway,
+  get_honda_accord_lead_departure_tune,
+  get_toyota_prius_stopped_lead_obstacle_bias,
+  get_toyota_rav4_tss2_lead_departure_tune,
   get_toyota_rav4_tss2_early_lead_cap,
   get_toyota_sienna_post_departure_restop_cap,
   is_toyota_rav4_tss2_radar_follow_lead,
@@ -91,6 +94,28 @@ def test_mpc_duplicate_lead_filters_do_not_cross_contaminate_tracks():
   assert mpc.duplicate_lead_v_filters[1].x == pytest.approx(28.0)
 
 
+def test_prius_stopped_lead_obstacle_bias_is_small_and_vehicle_specific():
+  prius = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_PRIUS)
+  other = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  stopped_lead = make_lead(status=True, d_rel=18.0, v_lead=0.2, model_prob=0.99)
+
+  bias = get_toyota_prius_stopped_lead_obstacle_bias(prius, stopped_lead, v_ego=8.0)
+  assert 0.0 < bias < 1.5
+  assert get_toyota_prius_stopped_lead_obstacle_bias(other, stopped_lead, v_ego=8.0) == pytest.approx(0.0)
+  assert get_toyota_prius_stopped_lead_obstacle_bias(
+    prius, make_lead(status=True, d_rel=18.0, v_lead=4.0), v_ego=8.0,
+  ) == pytest.approx(0.0)
+
+
+def test_prius_stopped_lead_obstacle_bias_does_not_apply_at_standstill_or_to_departures():
+  prius = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_PRIUS)
+  stopped_lead = make_lead(status=True, d_rel=4.0, v_lead=0.0, model_prob=0.99)
+  departing_lead = make_lead(status=True, d_rel=18.0, v_lead=2.0, model_prob=0.99)
+
+  assert get_toyota_prius_stopped_lead_obstacle_bias(prius, stopped_lead, v_ego=0.0) == pytest.approx(0.0)
+  assert get_toyota_prius_stopped_lead_obstacle_bias(prius, departing_lead, v_ego=8.0) == pytest.approx(0.0)
+
+
 def test_mpc_duplicate_vision_filter_smooths_distance_jumps_per_track():
   mpc = LongitudinalMpc()
   mpc.set_cur_state(27.0, 0.0)
@@ -2479,6 +2504,55 @@ def test_route_251682_rav4_confirmed_depart_adds_bounded_accel_assist():
   assert floor <= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL
 
 
+def test_honda_accord_lead_departure_assist_is_stronger_but_vehicle_scoped():
+  accord = CarInterface.get_non_essential_params(CAR.HONDA_ACCORD)
+  civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(accord, init_v=0.0)
+  lead = make_lead(
+    status=True,
+    d_rel=7.7,
+    v_lead=2.0,
+    a_lead=1.79,
+    radar=False,
+    model_prob=1.0,
+  )
+
+  accord_floor = planner.get_lead_depart_accel_floor(lead, v_ego=0.0, model_desired_accel=0.44)
+  civic_floor = LongitudinalPlanner(civic, init_v=0.0).get_lead_depart_accel_floor(
+    lead, v_ego=0.0, model_desired_accel=0.44,
+  )
+
+  assert get_honda_accord_lead_departure_tune(accord) is not None
+  assert get_honda_accord_lead_departure_tune(civic) is None
+  assert accord_floor > civic_floor
+  assert accord_floor <= get_honda_accord_lead_departure_tune(accord)[0]
+
+
+def test_rav4_tss2_lead_departure_assist_is_vehicle_scoped():
+  rav4 = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
+  civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  lead = make_lead(
+    status=True,
+    d_rel=7.7,
+    v_lead=2.0,
+    a_lead=1.79,
+    radar=False,
+    model_prob=1.0,
+  )
+
+  rav4_floor = LongitudinalPlanner(rav4, init_v=0.0).get_lead_depart_accel_floor(
+    lead, v_ego=0.0, model_desired_accel=0.44,
+  )
+  civic_floor = LongitudinalPlanner(civic, init_v=0.0).get_lead_depart_accel_floor(
+    lead, v_ego=0.0, model_desired_accel=0.44,
+  )
+
+  assert get_toyota_rav4_tss2_lead_departure_tune(rav4) is not None
+  assert get_toyota_rav4_tss2_lead_departure_tune(civic) is None
+  assert rav4_floor > civic_floor
+  assert rav4_floor == pytest.approx(0.64)
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
 def test_standstill_depart_accel_hold_reuses_floor_through_softening_lead_delta(model_version):
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)

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_4436222918597397805) {
-   out_4436222918597397805[0] = delta_x[0] + nom_x[0];
-   out_4436222918597397805[1] = delta_x[1] + nom_x[1];
-   out_4436222918597397805[2] = delta_x[2] + nom_x[2];
-   out_4436222918597397805[3] = delta_x[3] + nom_x[3];
-   out_4436222918597397805[4] = delta_x[4] + nom_x[4];
-   out_4436222918597397805[5] = delta_x[5] + nom_x[5];
-   out_4436222918597397805[6] = delta_x[6] + nom_x[6];
-   out_4436222918597397805[7] = delta_x[7] + nom_x[7];
-   out_4436222918597397805[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_8339538066892951453) {
+   out_8339538066892951453[0] = delta_x[0] + nom_x[0];
+   out_8339538066892951453[1] = delta_x[1] + nom_x[1];
+   out_8339538066892951453[2] = delta_x[2] + nom_x[2];
+   out_8339538066892951453[3] = delta_x[3] + nom_x[3];
+   out_8339538066892951453[4] = delta_x[4] + nom_x[4];
+   out_8339538066892951453[5] = delta_x[5] + nom_x[5];
+   out_8339538066892951453[6] = delta_x[6] + nom_x[6];
+   out_8339538066892951453[7] = delta_x[7] + nom_x[7];
+   out_8339538066892951453[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_5455437002668972894) {
-   out_5455437002668972894[0] = -nom_x[0] + true_x[0];
-   out_5455437002668972894[1] = -nom_x[1] + true_x[1];
-   out_5455437002668972894[2] = -nom_x[2] + true_x[2];
-   out_5455437002668972894[3] = -nom_x[3] + true_x[3];
-   out_5455437002668972894[4] = -nom_x[4] + true_x[4];
-   out_5455437002668972894[5] = -nom_x[5] + true_x[5];
-   out_5455437002668972894[6] = -nom_x[6] + true_x[6];
-   out_5455437002668972894[7] = -nom_x[7] + true_x[7];
-   out_5455437002668972894[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_1869575881362213489) {
+   out_1869575881362213489[0] = -nom_x[0] + true_x[0];
+   out_1869575881362213489[1] = -nom_x[1] + true_x[1];
+   out_1869575881362213489[2] = -nom_x[2] + true_x[2];
+   out_1869575881362213489[3] = -nom_x[3] + true_x[3];
+   out_1869575881362213489[4] = -nom_x[4] + true_x[4];
+   out_1869575881362213489[5] = -nom_x[5] + true_x[5];
+   out_1869575881362213489[6] = -nom_x[6] + true_x[6];
+   out_1869575881362213489[7] = -nom_x[7] + true_x[7];
+   out_1869575881362213489[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_210375879520347438) {
-   out_210375879520347438[0] = 1.0;
-   out_210375879520347438[1] = 0.0;
-   out_210375879520347438[2] = 0.0;
-   out_210375879520347438[3] = 0.0;
-   out_210375879520347438[4] = 0.0;
-   out_210375879520347438[5] = 0.0;
-   out_210375879520347438[6] = 0.0;
-   out_210375879520347438[7] = 0.0;
-   out_210375879520347438[8] = 0.0;
-   out_210375879520347438[9] = 0.0;
-   out_210375879520347438[10] = 1.0;
-   out_210375879520347438[11] = 0.0;
-   out_210375879520347438[12] = 0.0;
-   out_210375879520347438[13] = 0.0;
-   out_210375879520347438[14] = 0.0;
-   out_210375879520347438[15] = 0.0;
-   out_210375879520347438[16] = 0.0;
-   out_210375879520347438[17] = 0.0;
-   out_210375879520347438[18] = 0.0;
-   out_210375879520347438[19] = 0.0;
-   out_210375879520347438[20] = 1.0;
-   out_210375879520347438[21] = 0.0;
-   out_210375879520347438[22] = 0.0;
-   out_210375879520347438[23] = 0.0;
-   out_210375879520347438[24] = 0.0;
-   out_210375879520347438[25] = 0.0;
-   out_210375879520347438[26] = 0.0;
-   out_210375879520347438[27] = 0.0;
-   out_210375879520347438[28] = 0.0;
-   out_210375879520347438[29] = 0.0;
-   out_210375879520347438[30] = 1.0;
-   out_210375879520347438[31] = 0.0;
-   out_210375879520347438[32] = 0.0;
-   out_210375879520347438[33] = 0.0;
-   out_210375879520347438[34] = 0.0;
-   out_210375879520347438[35] = 0.0;
-   out_210375879520347438[36] = 0.0;
-   out_210375879520347438[37] = 0.0;
-   out_210375879520347438[38] = 0.0;
-   out_210375879520347438[39] = 0.0;
-   out_210375879520347438[40] = 1.0;
-   out_210375879520347438[41] = 0.0;
-   out_210375879520347438[42] = 0.0;
-   out_210375879520347438[43] = 0.0;
-   out_210375879520347438[44] = 0.0;
-   out_210375879520347438[45] = 0.0;
-   out_210375879520347438[46] = 0.0;
-   out_210375879520347438[47] = 0.0;
-   out_210375879520347438[48] = 0.0;
-   out_210375879520347438[49] = 0.0;
-   out_210375879520347438[50] = 1.0;
-   out_210375879520347438[51] = 0.0;
-   out_210375879520347438[52] = 0.0;
-   out_210375879520347438[53] = 0.0;
-   out_210375879520347438[54] = 0.0;
-   out_210375879520347438[55] = 0.0;
-   out_210375879520347438[56] = 0.0;
-   out_210375879520347438[57] = 0.0;
-   out_210375879520347438[58] = 0.0;
-   out_210375879520347438[59] = 0.0;
-   out_210375879520347438[60] = 1.0;
-   out_210375879520347438[61] = 0.0;
-   out_210375879520347438[62] = 0.0;
-   out_210375879520347438[63] = 0.0;
-   out_210375879520347438[64] = 0.0;
-   out_210375879520347438[65] = 0.0;
-   out_210375879520347438[66] = 0.0;
-   out_210375879520347438[67] = 0.0;
-   out_210375879520347438[68] = 0.0;
-   out_210375879520347438[69] = 0.0;
-   out_210375879520347438[70] = 1.0;
-   out_210375879520347438[71] = 0.0;
-   out_210375879520347438[72] = 0.0;
-   out_210375879520347438[73] = 0.0;
-   out_210375879520347438[74] = 0.0;
-   out_210375879520347438[75] = 0.0;
-   out_210375879520347438[76] = 0.0;
-   out_210375879520347438[77] = 0.0;
-   out_210375879520347438[78] = 0.0;
-   out_210375879520347438[79] = 0.0;
-   out_210375879520347438[80] = 1.0;
+void H_mod_fun(double *state, double *out_4134445094065349854) {
+   out_4134445094065349854[0] = 1.0;
+   out_4134445094065349854[1] = 0.0;
+   out_4134445094065349854[2] = 0.0;
+   out_4134445094065349854[3] = 0.0;
+   out_4134445094065349854[4] = 0.0;
+   out_4134445094065349854[5] = 0.0;
+   out_4134445094065349854[6] = 0.0;
+   out_4134445094065349854[7] = 0.0;
+   out_4134445094065349854[8] = 0.0;
+   out_4134445094065349854[9] = 0.0;
+   out_4134445094065349854[10] = 1.0;
+   out_4134445094065349854[11] = 0.0;
+   out_4134445094065349854[12] = 0.0;
+   out_4134445094065349854[13] = 0.0;
+   out_4134445094065349854[14] = 0.0;
+   out_4134445094065349854[15] = 0.0;
+   out_4134445094065349854[16] = 0.0;
+   out_4134445094065349854[17] = 0.0;
+   out_4134445094065349854[18] = 0.0;
+   out_4134445094065349854[19] = 0.0;
+   out_4134445094065349854[20] = 1.0;
+   out_4134445094065349854[21] = 0.0;
+   out_4134445094065349854[22] = 0.0;
+   out_4134445094065349854[23] = 0.0;
+   out_4134445094065349854[24] = 0.0;
+   out_4134445094065349854[25] = 0.0;
+   out_4134445094065349854[26] = 0.0;
+   out_4134445094065349854[27] = 0.0;
+   out_4134445094065349854[28] = 0.0;
+   out_4134445094065349854[29] = 0.0;
+   out_4134445094065349854[30] = 1.0;
+   out_4134445094065349854[31] = 0.0;
+   out_4134445094065349854[32] = 0.0;
+   out_4134445094065349854[33] = 0.0;
+   out_4134445094065349854[34] = 0.0;
+   out_4134445094065349854[35] = 0.0;
+   out_4134445094065349854[36] = 0.0;
+   out_4134445094065349854[37] = 0.0;
+   out_4134445094065349854[38] = 0.0;
+   out_4134445094065349854[39] = 0.0;
+   out_4134445094065349854[40] = 1.0;
+   out_4134445094065349854[41] = 0.0;
+   out_4134445094065349854[42] = 0.0;
+   out_4134445094065349854[43] = 0.0;
+   out_4134445094065349854[44] = 0.0;
+   out_4134445094065349854[45] = 0.0;
+   out_4134445094065349854[46] = 0.0;
+   out_4134445094065349854[47] = 0.0;
+   out_4134445094065349854[48] = 0.0;
+   out_4134445094065349854[49] = 0.0;
+   out_4134445094065349854[50] = 1.0;
+   out_4134445094065349854[51] = 0.0;
+   out_4134445094065349854[52] = 0.0;
+   out_4134445094065349854[53] = 0.0;
+   out_4134445094065349854[54] = 0.0;
+   out_4134445094065349854[55] = 0.0;
+   out_4134445094065349854[56] = 0.0;
+   out_4134445094065349854[57] = 0.0;
+   out_4134445094065349854[58] = 0.0;
+   out_4134445094065349854[59] = 0.0;
+   out_4134445094065349854[60] = 1.0;
+   out_4134445094065349854[61] = 0.0;
+   out_4134445094065349854[62] = 0.0;
+   out_4134445094065349854[63] = 0.0;
+   out_4134445094065349854[64] = 0.0;
+   out_4134445094065349854[65] = 0.0;
+   out_4134445094065349854[66] = 0.0;
+   out_4134445094065349854[67] = 0.0;
+   out_4134445094065349854[68] = 0.0;
+   out_4134445094065349854[69] = 0.0;
+   out_4134445094065349854[70] = 1.0;
+   out_4134445094065349854[71] = 0.0;
+   out_4134445094065349854[72] = 0.0;
+   out_4134445094065349854[73] = 0.0;
+   out_4134445094065349854[74] = 0.0;
+   out_4134445094065349854[75] = 0.0;
+   out_4134445094065349854[76] = 0.0;
+   out_4134445094065349854[77] = 0.0;
+   out_4134445094065349854[78] = 0.0;
+   out_4134445094065349854[79] = 0.0;
+   out_4134445094065349854[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_467846126367206141) {
-   out_467846126367206141[0] = state[0];
-   out_467846126367206141[1] = state[1];
-   out_467846126367206141[2] = state[2];
-   out_467846126367206141[3] = state[3];
-   out_467846126367206141[4] = state[4];
-   out_467846126367206141[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_467846126367206141[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_467846126367206141[7] = state[7];
-   out_467846126367206141[8] = state[8];
+void f_fun(double *state, double dt, double *out_1187519494066628558) {
+   out_1187519494066628558[0] = state[0];
+   out_1187519494066628558[1] = state[1];
+   out_1187519494066628558[2] = state[2];
+   out_1187519494066628558[3] = state[3];
+   out_1187519494066628558[4] = state[4];
+   out_1187519494066628558[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_1187519494066628558[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_1187519494066628558[7] = state[7];
+   out_1187519494066628558[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_3146439140780538523) {
-   out_3146439140780538523[0] = 1;
-   out_3146439140780538523[1] = 0;
-   out_3146439140780538523[2] = 0;
-   out_3146439140780538523[3] = 0;
-   out_3146439140780538523[4] = 0;
-   out_3146439140780538523[5] = 0;
-   out_3146439140780538523[6] = 0;
-   out_3146439140780538523[7] = 0;
-   out_3146439140780538523[8] = 0;
-   out_3146439140780538523[9] = 0;
-   out_3146439140780538523[10] = 1;
-   out_3146439140780538523[11] = 0;
-   out_3146439140780538523[12] = 0;
-   out_3146439140780538523[13] = 0;
-   out_3146439140780538523[14] = 0;
-   out_3146439140780538523[15] = 0;
-   out_3146439140780538523[16] = 0;
-   out_3146439140780538523[17] = 0;
-   out_3146439140780538523[18] = 0;
-   out_3146439140780538523[19] = 0;
-   out_3146439140780538523[20] = 1;
-   out_3146439140780538523[21] = 0;
-   out_3146439140780538523[22] = 0;
-   out_3146439140780538523[23] = 0;
-   out_3146439140780538523[24] = 0;
-   out_3146439140780538523[25] = 0;
-   out_3146439140780538523[26] = 0;
-   out_3146439140780538523[27] = 0;
-   out_3146439140780538523[28] = 0;
-   out_3146439140780538523[29] = 0;
-   out_3146439140780538523[30] = 1;
-   out_3146439140780538523[31] = 0;
-   out_3146439140780538523[32] = 0;
-   out_3146439140780538523[33] = 0;
-   out_3146439140780538523[34] = 0;
-   out_3146439140780538523[35] = 0;
-   out_3146439140780538523[36] = 0;
-   out_3146439140780538523[37] = 0;
-   out_3146439140780538523[38] = 0;
-   out_3146439140780538523[39] = 0;
-   out_3146439140780538523[40] = 1;
-   out_3146439140780538523[41] = 0;
-   out_3146439140780538523[42] = 0;
-   out_3146439140780538523[43] = 0;
-   out_3146439140780538523[44] = 0;
-   out_3146439140780538523[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_3146439140780538523[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_3146439140780538523[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_3146439140780538523[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_3146439140780538523[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_3146439140780538523[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_3146439140780538523[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_3146439140780538523[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_3146439140780538523[53] = -9.8100000000000005*dt;
-   out_3146439140780538523[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_3146439140780538523[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_3146439140780538523[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_3146439140780538523[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_3146439140780538523[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_3146439140780538523[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_3146439140780538523[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_3146439140780538523[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_3146439140780538523[62] = 0;
-   out_3146439140780538523[63] = 0;
-   out_3146439140780538523[64] = 0;
-   out_3146439140780538523[65] = 0;
-   out_3146439140780538523[66] = 0;
-   out_3146439140780538523[67] = 0;
-   out_3146439140780538523[68] = 0;
-   out_3146439140780538523[69] = 0;
-   out_3146439140780538523[70] = 1;
-   out_3146439140780538523[71] = 0;
-   out_3146439140780538523[72] = 0;
-   out_3146439140780538523[73] = 0;
-   out_3146439140780538523[74] = 0;
-   out_3146439140780538523[75] = 0;
-   out_3146439140780538523[76] = 0;
-   out_3146439140780538523[77] = 0;
-   out_3146439140780538523[78] = 0;
-   out_3146439140780538523[79] = 0;
-   out_3146439140780538523[80] = 1;
+void F_fun(double *state, double dt, double *out_638221139616797346) {
+   out_638221139616797346[0] = 1;
+   out_638221139616797346[1] = 0;
+   out_638221139616797346[2] = 0;
+   out_638221139616797346[3] = 0;
+   out_638221139616797346[4] = 0;
+   out_638221139616797346[5] = 0;
+   out_638221139616797346[6] = 0;
+   out_638221139616797346[7] = 0;
+   out_638221139616797346[8] = 0;
+   out_638221139616797346[9] = 0;
+   out_638221139616797346[10] = 1;
+   out_638221139616797346[11] = 0;
+   out_638221139616797346[12] = 0;
+   out_638221139616797346[13] = 0;
+   out_638221139616797346[14] = 0;
+   out_638221139616797346[15] = 0;
+   out_638221139616797346[16] = 0;
+   out_638221139616797346[17] = 0;
+   out_638221139616797346[18] = 0;
+   out_638221139616797346[19] = 0;
+   out_638221139616797346[20] = 1;
+   out_638221139616797346[21] = 0;
+   out_638221139616797346[22] = 0;
+   out_638221139616797346[23] = 0;
+   out_638221139616797346[24] = 0;
+   out_638221139616797346[25] = 0;
+   out_638221139616797346[26] = 0;
+   out_638221139616797346[27] = 0;
+   out_638221139616797346[28] = 0;
+   out_638221139616797346[29] = 0;
+   out_638221139616797346[30] = 1;
+   out_638221139616797346[31] = 0;
+   out_638221139616797346[32] = 0;
+   out_638221139616797346[33] = 0;
+   out_638221139616797346[34] = 0;
+   out_638221139616797346[35] = 0;
+   out_638221139616797346[36] = 0;
+   out_638221139616797346[37] = 0;
+   out_638221139616797346[38] = 0;
+   out_638221139616797346[39] = 0;
+   out_638221139616797346[40] = 1;
+   out_638221139616797346[41] = 0;
+   out_638221139616797346[42] = 0;
+   out_638221139616797346[43] = 0;
+   out_638221139616797346[44] = 0;
+   out_638221139616797346[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_638221139616797346[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_638221139616797346[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_638221139616797346[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_638221139616797346[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_638221139616797346[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_638221139616797346[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_638221139616797346[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_638221139616797346[53] = -9.8100000000000005*dt;
+   out_638221139616797346[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_638221139616797346[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_638221139616797346[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_638221139616797346[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_638221139616797346[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_638221139616797346[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_638221139616797346[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_638221139616797346[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_638221139616797346[62] = 0;
+   out_638221139616797346[63] = 0;
+   out_638221139616797346[64] = 0;
+   out_638221139616797346[65] = 0;
+   out_638221139616797346[66] = 0;
+   out_638221139616797346[67] = 0;
+   out_638221139616797346[68] = 0;
+   out_638221139616797346[69] = 0;
+   out_638221139616797346[70] = 1;
+   out_638221139616797346[71] = 0;
+   out_638221139616797346[72] = 0;
+   out_638221139616797346[73] = 0;
+   out_638221139616797346[74] = 0;
+   out_638221139616797346[75] = 0;
+   out_638221139616797346[76] = 0;
+   out_638221139616797346[77] = 0;
+   out_638221139616797346[78] = 0;
+   out_638221139616797346[79] = 0;
+   out_638221139616797346[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_3524127915259881655) {
-   out_3524127915259881655[0] = state[6];
+void h_25(double *state, double *unused, double *out_6007362445110034756) {
+   out_6007362445110034756[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_3733424518165712586) {
-   out_3733424518165712586[0] = 0;
-   out_3733424518165712586[1] = 0;
-   out_3733424518165712586[2] = 0;
-   out_3733424518165712586[3] = 0;
-   out_3733424518165712586[4] = 0;
-   out_3733424518165712586[5] = 0;
-   out_3733424518165712586[6] = 1;
-   out_3733424518165712586[7] = 0;
-   out_3733424518165712586[8] = 0;
+void H_25(double *state, double *unused, double *out_5119110753731964525) {
+   out_5119110753731964525[0] = 0;
+   out_5119110753731964525[1] = 0;
+   out_5119110753731964525[2] = 0;
+   out_5119110753731964525[3] = 0;
+   out_5119110753731964525[4] = 0;
+   out_5119110753731964525[5] = 0;
+   out_5119110753731964525[6] = 1;
+   out_5119110753731964525[7] = 0;
+   out_5119110753731964525[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_5258226883159248845) {
-   out_5258226883159248845[0] = state[4];
-   out_5258226883159248845[1] = state[5];
+void h_24(double *state, double *unused, double *out_5449620096162801389) {
+   out_5449620096162801389[0] = state[4];
+   out_5449620096162801389[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_2842147288425805515) {
-   out_2842147288425805515[0] = 0;
-   out_2842147288425805515[1] = 0;
-   out_2842147288425805515[2] = 0;
-   out_2842147288425805515[3] = 0;
-   out_2842147288425805515[4] = 1;
-   out_2842147288425805515[5] = 0;
-   out_2842147288425805515[6] = 0;
-   out_2842147288425805515[7] = 0;
-   out_2842147288425805515[8] = 0;
-   out_2842147288425805515[9] = 0;
-   out_2842147288425805515[10] = 0;
-   out_2842147288425805515[11] = 0;
-   out_2842147288425805515[12] = 0;
-   out_2842147288425805515[13] = 0;
-   out_2842147288425805515[14] = 1;
-   out_2842147288425805515[15] = 0;
-   out_2842147288425805515[16] = 0;
-   out_2842147288425805515[17] = 0;
+void H_24(double *state, double *unused, double *out_2946461154726464959) {
+   out_2946461154726464959[0] = 0;
+   out_2946461154726464959[1] = 0;
+   out_2946461154726464959[2] = 0;
+   out_2946461154726464959[3] = 0;
+   out_2946461154726464959[4] = 1;
+   out_2946461154726464959[5] = 0;
+   out_2946461154726464959[6] = 0;
+   out_2946461154726464959[7] = 0;
+   out_2946461154726464959[8] = 0;
+   out_2946461154726464959[9] = 0;
+   out_2946461154726464959[10] = 0;
+   out_2946461154726464959[11] = 0;
+   out_2946461154726464959[12] = 0;
+   out_2946461154726464959[13] = 0;
+   out_2946461154726464959[14] = 1;
+   out_2946461154726464959[15] = 0;
+   out_2946461154726464959[16] = 0;
+   out_2946461154726464959[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_2922709757646518101) {
-   out_2922709757646518101[0] = state[4];
+void h_30(double *state, double *unused, double *out_2369912916856012406) {
+   out_2369912916856012406[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_3183265823325904169) {
-   out_3183265823325904169[0] = 0;
-   out_3183265823325904169[1] = 0;
-   out_3183265823325904169[2] = 0;
-   out_3183265823325904169[3] = 0;
-   out_3183265823325904169[4] = 1;
-   out_3183265823325904169[5] = 0;
-   out_3183265823325904169[6] = 0;
-   out_3183265823325904169[7] = 0;
-   out_3183265823325904169[8] = 0;
+void H_30(double *state, double *unused, double *out_7637443712239213152) {
+   out_7637443712239213152[0] = 0;
+   out_7637443712239213152[1] = 0;
+   out_7637443712239213152[2] = 0;
+   out_7637443712239213152[3] = 0;
+   out_7637443712239213152[4] = 1;
+   out_7637443712239213152[5] = 0;
+   out_7637443712239213152[6] = 0;
+   out_7637443712239213152[7] = 0;
+   out_7637443712239213152[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_2078963510014327544) {
-   out_2078963510014327544[0] = state[7];
+void h_26(double *state, double *unused, double *out_6836290203325307661) {
+   out_6836290203325307661[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_7474927837039768810) {
-   out_7474927837039768810[0] = 0;
-   out_7474927837039768810[1] = 0;
-   out_7474927837039768810[2] = 0;
-   out_7474927837039768810[3] = 0;
-   out_7474927837039768810[4] = 0;
-   out_7474927837039768810[5] = 0;
-   out_7474927837039768810[6] = 0;
-   out_7474927837039768810[7] = 1;
-   out_7474927837039768810[8] = 0;
+void H_26(double *state, double *unused, double *out_1377607434857908301) {
+   out_1377607434857908301[0] = 0;
+   out_1377607434857908301[1] = 0;
+   out_1377607434857908301[2] = 0;
+   out_1377607434857908301[3] = 0;
+   out_1377607434857908301[4] = 0;
+   out_1377607434857908301[5] = 0;
+   out_1377607434857908301[6] = 0;
+   out_1377607434857908301[7] = 1;
+   out_1377607434857908301[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_9006985404715807625) {
-   out_9006985404715807625[0] = state[3];
+void h_27(double *state, double *unused, double *out_2710474474758657570) {
+   out_2710474474758657570[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_1008502511525479258) {
-   out_1008502511525479258[0] = 0;
-   out_1008502511525479258[1] = 0;
-   out_1008502511525479258[2] = 0;
-   out_1008502511525479258[3] = 1;
-   out_1008502511525479258[4] = 0;
-   out_1008502511525479258[5] = 0;
-   out_1008502511525479258[6] = 0;
-   out_1008502511525479258[7] = 0;
-   out_1008502511525479258[8] = 0;
+void H_27(double *state, double *unused, double *out_8585706290286395247) {
+   out_8585706290286395247[0] = 0;
+   out_8585706290286395247[1] = 0;
+   out_8585706290286395247[2] = 0;
+   out_8585706290286395247[3] = 1;
+   out_8585706290286395247[4] = 0;
+   out_8585706290286395247[5] = 0;
+   out_8585706290286395247[6] = 0;
+   out_8585706290286395247[7] = 0;
+   out_8585706290286395247[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_1979199904744855666) {
-   out_1979199904744855666[0] = state[1];
+void h_29(double *state, double *unused, double *out_1542363084005055587) {
+   out_1542363084005055587[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_704860215344071775) {
-   out_704860215344071775[0] = 0;
-   out_704860215344071775[1] = 1;
-   out_704860215344071775[2] = 0;
-   out_704860215344071775[3] = 0;
-   out_704860215344071775[4] = 0;
-   out_704860215344071775[5] = 0;
-   out_704860215344071775[6] = 0;
-   out_704860215344071775[7] = 0;
-   out_704860215344071775[8] = 0;
+void H_29(double *state, double *unused, double *out_8147675056553605336) {
+   out_8147675056553605336[0] = 0;
+   out_8147675056553605336[1] = 1;
+   out_8147675056553605336[2] = 0;
+   out_8147675056553605336[3] = 0;
+   out_8147675056553605336[4] = 0;
+   out_8147675056553605336[5] = 0;
+   out_8147675056553605336[6] = 0;
+   out_8147675056553605336[7] = 0;
+   out_8147675056553605336[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_6835353348185046340) {
-   out_6835353348185046340[0] = state[0];
+void h_28(double *state, double *unused, double *out_7712147742419503215) {
+   out_7712147742419503215[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_5787259232413602349) {
-   out_5787259232413602349[0] = 1;
-   out_5787259232413602349[1] = 0;
-   out_5787259232413602349[2] = 0;
-   out_5787259232413602349[3] = 0;
-   out_5787259232413602349[4] = 0;
-   out_5787259232413602349[5] = 0;
-   out_5787259232413602349[6] = 0;
-   out_5787259232413602349[7] = 0;
-   out_5787259232413602349[8] = 0;
+void H_28(double *state, double *unused, double *out_3065276039484074762) {
+   out_3065276039484074762[0] = 1;
+   out_3065276039484074762[1] = 0;
+   out_3065276039484074762[2] = 0;
+   out_3065276039484074762[3] = 0;
+   out_3065276039484074762[4] = 0;
+   out_3065276039484074762[5] = 0;
+   out_3065276039484074762[6] = 0;
+   out_3065276039484074762[7] = 0;
+   out_3065276039484074762[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_4974759307888764822) {
-   out_4974759307888764822[0] = state[8];
+void h_31(double *state, double *unused, double *out_6282556507394540645) {
+   out_6282556507394540645[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_3702778556288752158) {
-   out_3702778556288752158[0] = 0;
-   out_3702778556288752158[1] = 0;
-   out_3702778556288752158[2] = 0;
-   out_3702778556288752158[3] = 0;
-   out_3702778556288752158[4] = 0;
-   out_3702778556288752158[5] = 0;
-   out_3702778556288752158[6] = 0;
-   out_3702778556288752158[7] = 0;
-   out_3702778556288752158[8] = 1;
+void H_31(double *state, double *unused, double *out_5149756715608924953) {
+   out_5149756715608924953[0] = 0;
+   out_5149756715608924953[1] = 0;
+   out_5149756715608924953[2] = 0;
+   out_5149756715608924953[3] = 0;
+   out_5149756715608924953[4] = 0;
+   out_5149756715608924953[5] = 0;
+   out_5149756715608924953[6] = 0;
+   out_5149756715608924953[7] = 0;
+   out_5149756715608924953[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_4436222918597397805) {
-  err_fun(nom_x, delta_x, out_4436222918597397805);
+void car_err_fun(double *nom_x, double *delta_x, double *out_8339538066892951453) {
+  err_fun(nom_x, delta_x, out_8339538066892951453);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_5455437002668972894) {
-  inv_err_fun(nom_x, true_x, out_5455437002668972894);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_1869575881362213489) {
+  inv_err_fun(nom_x, true_x, out_1869575881362213489);
 }
-void car_H_mod_fun(double *state, double *out_210375879520347438) {
-  H_mod_fun(state, out_210375879520347438);
+void car_H_mod_fun(double *state, double *out_4134445094065349854) {
+  H_mod_fun(state, out_4134445094065349854);
 }
-void car_f_fun(double *state, double dt, double *out_467846126367206141) {
-  f_fun(state,  dt, out_467846126367206141);
+void car_f_fun(double *state, double dt, double *out_1187519494066628558) {
+  f_fun(state,  dt, out_1187519494066628558);
 }
-void car_F_fun(double *state, double dt, double *out_3146439140780538523) {
-  F_fun(state,  dt, out_3146439140780538523);
+void car_F_fun(double *state, double dt, double *out_638221139616797346) {
+  F_fun(state,  dt, out_638221139616797346);
 }
-void car_h_25(double *state, double *unused, double *out_3524127915259881655) {
-  h_25(state, unused, out_3524127915259881655);
+void car_h_25(double *state, double *unused, double *out_6007362445110034756) {
+  h_25(state, unused, out_6007362445110034756);
 }
-void car_H_25(double *state, double *unused, double *out_3733424518165712586) {
-  H_25(state, unused, out_3733424518165712586);
+void car_H_25(double *state, double *unused, double *out_5119110753731964525) {
+  H_25(state, unused, out_5119110753731964525);
 }
-void car_h_24(double *state, double *unused, double *out_5258226883159248845) {
-  h_24(state, unused, out_5258226883159248845);
+void car_h_24(double *state, double *unused, double *out_5449620096162801389) {
+  h_24(state, unused, out_5449620096162801389);
 }
-void car_H_24(double *state, double *unused, double *out_2842147288425805515) {
-  H_24(state, unused, out_2842147288425805515);
+void car_H_24(double *state, double *unused, double *out_2946461154726464959) {
+  H_24(state, unused, out_2946461154726464959);
 }
-void car_h_30(double *state, double *unused, double *out_2922709757646518101) {
-  h_30(state, unused, out_2922709757646518101);
+void car_h_30(double *state, double *unused, double *out_2369912916856012406) {
+  h_30(state, unused, out_2369912916856012406);
 }
-void car_H_30(double *state, double *unused, double *out_3183265823325904169) {
-  H_30(state, unused, out_3183265823325904169);
+void car_H_30(double *state, double *unused, double *out_7637443712239213152) {
+  H_30(state, unused, out_7637443712239213152);
 }
-void car_h_26(double *state, double *unused, double *out_2078963510014327544) {
-  h_26(state, unused, out_2078963510014327544);
+void car_h_26(double *state, double *unused, double *out_6836290203325307661) {
+  h_26(state, unused, out_6836290203325307661);
 }
-void car_H_26(double *state, double *unused, double *out_7474927837039768810) {
-  H_26(state, unused, out_7474927837039768810);
+void car_H_26(double *state, double *unused, double *out_1377607434857908301) {
+  H_26(state, unused, out_1377607434857908301);
 }
-void car_h_27(double *state, double *unused, double *out_9006985404715807625) {
-  h_27(state, unused, out_9006985404715807625);
+void car_h_27(double *state, double *unused, double *out_2710474474758657570) {
+  h_27(state, unused, out_2710474474758657570);
 }
-void car_H_27(double *state, double *unused, double *out_1008502511525479258) {
-  H_27(state, unused, out_1008502511525479258);
+void car_H_27(double *state, double *unused, double *out_8585706290286395247) {
+  H_27(state, unused, out_8585706290286395247);
 }
-void car_h_29(double *state, double *unused, double *out_1979199904744855666) {
-  h_29(state, unused, out_1979199904744855666);
+void car_h_29(double *state, double *unused, double *out_1542363084005055587) {
+  h_29(state, unused, out_1542363084005055587);
 }
-void car_H_29(double *state, double *unused, double *out_704860215344071775) {
-  H_29(state, unused, out_704860215344071775);
+void car_H_29(double *state, double *unused, double *out_8147675056553605336) {
+  H_29(state, unused, out_8147675056553605336);
 }
-void car_h_28(double *state, double *unused, double *out_6835353348185046340) {
-  h_28(state, unused, out_6835353348185046340);
+void car_h_28(double *state, double *unused, double *out_7712147742419503215) {
+  h_28(state, unused, out_7712147742419503215);
 }
-void car_H_28(double *state, double *unused, double *out_5787259232413602349) {
-  H_28(state, unused, out_5787259232413602349);
+void car_H_28(double *state, double *unused, double *out_3065276039484074762) {
+  H_28(state, unused, out_3065276039484074762);
 }
-void car_h_31(double *state, double *unused, double *out_4974759307888764822) {
-  h_31(state, unused, out_4974759307888764822);
+void car_h_31(double *state, double *unused, double *out_6282556507394540645) {
+  h_31(state, unused, out_6282556507394540645);
 }
-void car_H_31(double *state, double *unused, double *out_3702778556288752158) {
-  H_31(state, unused, out_3702778556288752158);
+void car_H_31(double *state, double *unused, double *out_5149756715608924953) {
+  H_31(state, unused, out_5149756715608924953);
 }
 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_4436222918597397805);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_5455437002668972894);
-void car_H_mod_fun(double *state, double *out_210375879520347438);
-void car_f_fun(double *state, double dt, double *out_467846126367206141);
-void car_F_fun(double *state, double dt, double *out_3146439140780538523);
-void car_h_25(double *state, double *unused, double *out_3524127915259881655);
-void car_H_25(double *state, double *unused, double *out_3733424518165712586);
-void car_h_24(double *state, double *unused, double *out_5258226883159248845);
-void car_H_24(double *state, double *unused, double *out_2842147288425805515);
-void car_h_30(double *state, double *unused, double *out_2922709757646518101);
-void car_H_30(double *state, double *unused, double *out_3183265823325904169);
-void car_h_26(double *state, double *unused, double *out_2078963510014327544);
-void car_H_26(double *state, double *unused, double *out_7474927837039768810);
-void car_h_27(double *state, double *unused, double *out_9006985404715807625);
-void car_H_27(double *state, double *unused, double *out_1008502511525479258);
-void car_h_29(double *state, double *unused, double *out_1979199904744855666);
-void car_H_29(double *state, double *unused, double *out_704860215344071775);
-void car_h_28(double *state, double *unused, double *out_6835353348185046340);
-void car_H_28(double *state, double *unused, double *out_5787259232413602349);
-void car_h_31(double *state, double *unused, double *out_4974759307888764822);
-void car_H_31(double *state, double *unused, double *out_3702778556288752158);
+void car_err_fun(double *nom_x, double *delta_x, double *out_8339538066892951453);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_1869575881362213489);
+void car_H_mod_fun(double *state, double *out_4134445094065349854);
+void car_f_fun(double *state, double dt, double *out_1187519494066628558);
+void car_F_fun(double *state, double dt, double *out_638221139616797346);
+void car_h_25(double *state, double *unused, double *out_6007362445110034756);
+void car_H_25(double *state, double *unused, double *out_5119110753731964525);
+void car_h_24(double *state, double *unused, double *out_5449620096162801389);
+void car_H_24(double *state, double *unused, double *out_2946461154726464959);
+void car_h_30(double *state, double *unused, double *out_2369912916856012406);
+void car_H_30(double *state, double *unused, double *out_7637443712239213152);
+void car_h_26(double *state, double *unused, double *out_6836290203325307661);
+void car_H_26(double *state, double *unused, double *out_1377607434857908301);
+void car_h_27(double *state, double *unused, double *out_2710474474758657570);
+void car_H_27(double *state, double *unused, double *out_8585706290286395247);
+void car_h_29(double *state, double *unused, double *out_1542363084005055587);
+void car_H_29(double *state, double *unused, double *out_8147675056553605336);
+void car_h_28(double *state, double *unused, double *out_7712147742419503215);
+void car_H_28(double *state, double *unused, double *out_3065276039484074762);
+void car_h_31(double *state, double *unused, double *out_6282556507394540645);
+void car_H_31(double *state, double *unused, double *out_5149756715608924953);
 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_4409321269442810496);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3789556254182165506);
-void pose_H_mod_fun(double *state, double *out_3068405914716205428);
-void pose_f_fun(double *state, double dt, double *out_5515990809264108996);
-void pose_F_fun(double *state, double dt, double *out_1412435369220653896);
-void pose_h_4(double *state, double *unused, double *out_7211674724064901455);
-void pose_H_4(double *state, double *unused, double *out_8220924299755286163);
-void pose_h_10(double *state, double *unused, double *out_7091555343827122853);
-void pose_H_10(double *state, double *unused, double *out_3357399461201359920);
-void pose_h_13(double *state, double *unused, double *out_4463366606757688719);
-void pose_H_13(double *state, double *unused, double *out_7013545948621932652);
-void pose_h_14(double *state, double *unused, double *out_2594435200759890716);
-void pose_H_14(double *state, double *unused, double *out_6262578917614780924);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_8230036097863094971);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8741051770711475830);
+void pose_H_mod_fun(double *state, double *out_1900632188772248703);
+void pose_f_fun(double *state, double dt, double *out_3077738493587277525);
+void pose_F_fun(double *state, double dt, double *out_474426866180563475);
+void pose_h_4(double *state, double *unused, double *out_616527671886839495);
+void pose_H_4(double *state, double *unused, double *out_702684706889350599);
+void pose_h_10(double *state, double *unused, double *out_767369525646024651);
+void pose_H_10(double *state, double *unused, double *out_8347276505705953254);
+void pose_h_13(double *state, double *unused, double *out_7518652466578138352);
+void pose_H_13(double *state, double *unused, double *out_2509589118442982202);
+void pose_h_14(double *state, double *unused, double *out_7727744869207997497);
+void pose_H_14(double *state, double *unused, double *out_8183830522169091023);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }