Tesla

2026-08-07 09:15:51 +08:00 · 2026-05-04 19:12:20 -05:00
parent 7e4818916b
commit 7837a3c1aa
68 changed files with 1797 additions and 1377 deletions
@@ -270,6 +270,20 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"LongPitch", {PERSISTENT, BOOL, "1", "0", 2}},
    {"RemoteStartBootsComma", {PERSISTENT, BOOL, "0", "0"}},
    {"RemapCancelToDistance", {PERSISTENT, BOOL, "0", "0"}},
+    {"NAPAdaptiveAccel", {PERSISTENT, BOOL, "1", "1"}},
+    {"NAPFollowDistance", {PERSISTENT, INT, "4", "4"}},
+    {"NAPForcePreAP", {PERSISTENT, BOOL, "0", "0"}},
+    {"NAPPedalEnabled", {PERSISTENT, BOOL, "0", "0"}},
+    {"NAPPedalCanBus", {PERSISTENT, INT, "2", "2"}},
+    {"NAPPedalCalibDone", {PERSISTENT, BOOL, "0", "0"}},
+    {"NAPPedalCalibMin", {PERSISTENT, FLOAT, "-3.0", "-3.0"}},
+    {"NAPPedalCalibMax", {PERSISTENT, FLOAT, "99.6", "99.6"}},
+    {"NAPPedalCalibFactor", {PERSISTENT, FLOAT, "1.0", "1.0"}},
+    {"NAPPedalCalibZero", {PERSISTENT, FLOAT, "0.0", "0.0"}},
+    {"NAPPedalProfile", {PERSISTENT, INT, "4", "4"}},
+    {"NAPRadarBehindNosecone", {PERSISTENT, BOOL, "0", "0"}},
+    {"NAPRadarEnabled", {PERSISTENT, BOOL, "1", "1"}},
+    {"NAPRadarOffset", {PERSISTENT, FLOAT, "0.0", "0.0"}},
    {"ForceAutoTune", {PERSISTENT, BOOL, "0", "0", 3}},
    {"ForceAutoTuneOff", {PERSISTENT, BOOL, "1", "0", 2}},
    {"ForceFingerprint", {PERSISTENT, BOOL, "0", "0", 2}},
@@ -635,6 +635,7 @@ struct CarParams {
    fcaGiorgio @32;
    rivian @33;
    volkswagenMeb @34;
+    teslaPreap @35;
  }

  enum SteerControlType {
@@ -211,6 +211,11 @@ def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_mu
  disable_fw_cache = os.environ.get('DISABLE_FW_CACHE', False)
  ecu_rx_addrs = set()

+  if not fixed_fingerprint and Params().get_bool("NAPForcePreAP"):
+    fixed_fingerprint = "TESLA_MODEL_S_PREAP"
+    skip_fw_query = True
+    carlog.warning("NAPForcePreAP enabled; forcing TESLA_MODEL_S_PREAP fingerprint")
+
  start_time = time.monotonic()
  if not skip_fw_query:
    if cached_params is not None and cached_params.brand != "mock" and len(cached_params.carFw) > 0 and \
@@ -247,6 +247,9 @@ class CarInterfaceBase(ABC):
          if 0x23 in fingerprint[0]:
            fp_ret.flags |= ToyotaStarPilotFlags.ZSS.value

+      elif platform.config.platform_str == "TESLA_MODEL_S_PREAP":
+        fp_ret.canUsePedal = True
+
    return fp_ret

  @staticmethod
@@ -4,15 +4,15 @@ from opendbc.car import Bus
 from opendbc.car.lateral import apply_steer_angle_limits_vm
 from opendbc.car.interfaces import CarControllerBase
 from opendbc.car.tesla.teslacan import TeslaCAN
-from opendbc.car.tesla.values import CarControllerParams
+from opendbc.car.tesla.preap.carcontroller import PreAPLongController, init_preap_can
+from opendbc.car.tesla.preap.stock_cc_spoofer import StockCCSpoofer
+from opendbc.car.tesla.values import CAR, CarControllerParams
 from opendbc.car.vehicle_model import VehicleModel


 def get_safety_CP():
-  # We use the TESLA_MODEL_Y platform for lateral limiting to match safety
-  # A Model 3 at 40 m/s using the Model Y limits sees a <0.3% difference in max angle (from curvature factor)
  from opendbc.car.tesla.interface import CarInterface
-  return CarInterface.get_non_essential_params("TESLA_MODEL_Y")
+  return CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP if getattr(get_safety_CP, "_preap", False) else CAR.TESLA_MODEL_Y)


 class CarController(CarControllerBase):
@@ -21,11 +21,23 @@ class CarController(CarControllerBase):
    self.apply_angle_last = 0
    self.packer = CANPacker(dbc_names[Bus.party])
    self.tesla_can = TeslaCAN(self.packer)
+    self.preap_long = None
+    self.stock_cc = None
+
+    if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
+      get_safety_CP._preap = True
+      self.tesla_can = init_preap_can(dbc_names)
+      self.preap_long = PreAPLongController()
+      self.stock_cc = StockCCSpoofer()

    # Vehicle model used for lateral limiting
    self.VM = VehicleModel(get_safety_CP())
+    get_safety_CP._preap = False

  def update(self, CC, CS, now_nanos, starpilot_toggles):
+    if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
+      return self._update_preap(CC, CS)
+
    actuators = CC.actuators
    can_sends = []

@@ -64,3 +76,42 @@ class CarController(CarControllerBase):

    self.frame += 1
    return new_actuators, can_sends
+
+  def _update_preap(self, CC, CS):
+    actuators = CC.actuators
+    can_sends = []
+    lat_active = CC.latActive and CS.hands_on_level < 3
+
+    if CC.cruiseControl.cancel and CS.cruiseEnabled:
+      CS.cruiseEnabled = False
+      CS.enableLongControl = False
+      CS.enableJustCC = False
+      CS.pedal_speed_kph = 0.0
+      CS.preap_cc_cancel_needed = True
+      if hasattr(CS, "engagement"):
+        CS.engagement.cruiseEnabled = False
+        CS.engagement.enableLongControl = False
+        CS.engagement.enableJustCC = False
+        CS.engagement.pending_enable = False
+        CS.engagement.pedal_speed_kph = 0.0
+
+    if self.frame % 2 == 0:
+      self.apply_angle_last = apply_steer_angle_limits_vm(
+        actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg,
+        lat_active, CarControllerParams, self.VM,
+      )
+      cntr = (self.frame // 2) % 16
+      can_sends.append(self.tesla_can.create_steering_control(cntr, self.apply_angle_last, lat_active))
+      can_sends.append(self.tesla_can.create_epas_control(cntr, 1))
+
+    if self.CP.openpilotLongitudinalControl and self.preap_long is not None:
+      can_sends.extend(self.preap_long.update(CC, CS, self.frame, self.tesla_can, CANBUS.party))
+
+    if self.stock_cc is not None:
+      can_sends.extend(self.stock_cc.update(CS, self.frame, self.tesla_can, CANBUS.party))
+
+    new_actuators = actuators.as_builder()
+    new_actuators.steeringAngleDeg = self.apply_angle_last
+
+    self.frame += 1
+    return new_actuators, can_sends
@@ -5,6 +5,10 @@ from opendbc.car import Bus, structs
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.interfaces import CarStateBase
 from opendbc.car.tesla.values import DBC, CANBUS, GEAR_MAP, STEER_THRESHOLD, CAR
+from opendbc.car.tesla.preap.carstate import get_preap_can_parsers, update_preap
+from opendbc.car.tesla.preap.engagement import PreAPEngagement
+from opendbc.car.tesla.preap.nap_conf import nap_conf
+from opendbc.car.tesla.preap.pedal_feedback import PedalFeedback

 ButtonType = structs.CarState.ButtonEvent.Type

@@ -13,7 +17,8 @@ class CarState(CarStateBase):
  def __init__(self, CP, FPCP):
    super().__init__(CP, FPCP)
    self.can_define = CANDefine(DBC[CP.carFingerprint][Bus.party])
-    self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"]
+    self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"] if CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP else \
+                          self.can_define.dv["DI_torque2"]["DI_gear"]

    self.autopark = False
    self.autopark_prev = False
@@ -21,6 +26,23 @@ class CarState(CarStateBase):

    self.hands_on_level = 0
    self.das_control = None
+    self.cruise_buttons = 0
+    self.prev_cruise_buttons = 0
+    self.msg_stw_actn_req = None
+    self.speed_units = "MPH"
+
+    if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
+      self.engagement = PreAPEngagement(nap_conf.double_pull_enabled, nap_conf.double_pull_window_ms)
+      self.cruiseEnabled = False
+      self.enableLongControl = False
+      self.enableJustCC = False
+      self.pedal_speed_kph = 0.0
+      self.preap_cc_cancel_needed = False
+      self.preap_cc_engage_needed = False
+      self.di_cruise_state = "OFF"
+      self.pedal = PedalFeedback()
+      self.pedal_interceptor_value = 0.0
+      self.pedal_timeout = True

  def update_autopark_state(self, autopark_state: str, cruise_enabled: bool):
    autopark_now = autopark_state in ("ACTIVE", "COMPLETE", "SELFPARK_STARTED")
@@ -32,6 +54,9 @@ class CarState(CarStateBase):
    self.cruise_enabled_prev = cruise_enabled

  def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
+    if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
+      return update_preap(self, can_parsers)
+
    cp_party = can_parsers[Bus.party]
    cp_ap_party = can_parsers[Bus.ap_party]
    ret = structs.CarState()
@@ -124,6 +149,8 @@ class CarState(CarStateBase):

  @staticmethod
  def get_can_parsers(CP):
+    if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
+      return get_preap_can_parsers(CP)
    return {
      Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party),
      Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party)
@@ -45,3 +45,22 @@ FW_VERSIONS = {
    ],
  },
 }
+
+FINGERPRINTS = {
+  CAR.TESLA_MODEL_S_PREAP: [
+    {
+      1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4,
+      309: 5, 325: 8, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4,
+      522: 8, 524: 8, 527: 8, 536: 8, 551: 4, 552: 2, 556: 8, 568: 8, 582: 5, 638: 8, 643: 8,
+      693: 8, 696: 8, 712: 8, 728: 8, 744: 8, 760: 8, 771: 2, 772: 8, 775: 8, 776: 8, 778: 8,
+      780: 2, 783: 8, 785: 8, 787: 8, 788: 8, 791: 8, 792: 8, 796: 2, 799: 8, 804: 8, 805: 8,
+      807: 8, 808: 1, 812: 8, 815: 8, 820: 8, 823: 8, 824: 8, 831: 8, 836: 8, 840: 8, 856: 4,
+      863: 8, 872: 8, 880: 8, 888: 8, 896: 8, 901: 6, 904: 3, 920: 8, 936: 8, 949: 8, 952: 8,
+      953: 6, 968: 8, 984: 8, 1000: 8, 1006: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1,
+      1034: 8, 1048: 1, 1064: 8, 1080: 8, 1281: 8, 1285: 8, 1332: 8, 1335: 8, 1362: 6, 1368: 8,
+      1412: 8, 1436: 8, 1456: 8, 1463: 8, 1476: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1617: 8,
+      1621: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8,
+      1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5,
+    },
+  ],
+}
@@ -2,17 +2,29 @@ from opendbc.car import get_safety_config, structs
 from opendbc.car.interfaces import CarInterfaceBase
 from opendbc.car.tesla.carcontroller import CarController
 from opendbc.car.tesla.carstate import CarState
+from opendbc.car.tesla.radar_interface import RadarInterface
 from opendbc.car.tesla.values import TeslaSafetyFlags, CAR
+from opendbc.car.tesla.preap.interface import get_preap_accel_limits, get_preap_params


 class CarInterface(CarInterfaceBase):
  CarState = CarState
  CarController = CarController
+  RadarInterface = RadarInterface
+
+  @staticmethod
+  def get_pid_accel_limits(CP, current_speed, cruise_speed):
+    if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
+      return get_preap_accel_limits(current_speed)
+    return CarInterfaceBase.get_pid_accel_limits(CP, current_speed, cruise_speed)

  @staticmethod
  def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
    ret.brand = "tesla"

+    if candidate == CAR.TESLA_MODEL_S_PREAP:
+      return get_preap_params(ret)
+
    ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla)]

    ret.steerLimitTimer = 0.4
@@ -60,6 +60,16 @@ class CAR(Platforms):
    [TeslaCarDocsHW4("Tesla Model X (with HW4) 2024")],
    CarSpecs(mass=2495., wheelbase=2.960, steerRatio=12.0),
  )
+  TESLA_MODEL_S_PREAP = TeslaPlatformConfig(
+    [CarDocs("Tesla Model S (Pre-AP) 2012-14", "All")],
+    CarSpecs(mass=2100., wheelbase=2.960, steerRatio=15.0),
+    {
+      Bus.party: 'tesla_can',
+      Bus.pt: 'tesla_can',
+      Bus.chassis: 'tesla_can',
+      Bus.radar: 'tesla_radar_bosch_generated',
+    },
+  )


 FW_QUERY_CONFIG = FwQueryConfig(
@@ -75,7 +85,8 @@ FW_QUERY_CONFIG = FwQueryConfig(

 class CANBUS:
  party = 0
-  vehicle = 1
+  radar = 1
+  vehicle = radar
  autopilot_party = 2


@@ -125,6 +136,25 @@ class TeslaFlags(IntFlag):
  LONG_CONTROL = 1


+class CruiseButtons:
+  IDLE = 0
+  CANCEL = 1
+  MAIN = 2
+  RES_ACCEL_2ND = 4
+  DECEL_2ND = 8
+  SET_ACCEL = 16
+  RES_ACCEL = 16
+  DECEL_SET = 32
+
+  @classmethod
+  def is_accel(cls, btn: int) -> bool:
+    return btn in (cls.RES_ACCEL, cls.RES_ACCEL_2ND)
+
+  @classmethod
+  def is_decel(cls, btn: int) -> bool:
+    return btn in (cls.DECEL_SET, cls.DECEL_2ND)
+
+
 DBC = CAR.create_dbc_map()

 STEER_THRESHOLD = 1
@@ -89,6 +89,8 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 "SUBARU_FORESTER_HYBRID" = "SUBARU_IMPREZA_2020"
 "SUBARU_LEGACY" = "SUBARU_OUTBACK"

+"TESLA_MODEL_S_PREAP" = "TESLA_MODEL_3"
+
 # Old subarus don't have much data guessing it's like low torque impreza"
 "SUBARU_OUTBACK_PREGLOBAL_2018" = "SUBARU_IMPREZA"
 "SUBARU_OUTBACK_PREGLOBAL" = "SUBARU_IMPREZA"
@@ -725,6 +725,15 @@ BO_ 780 DriverSeat: 8 XXX
 SG_ occupancyStatus : 16|3@1+ (1,0) [0|7] "" XXX
 SG_ buckleStatus : 19|2@1+ (1,0) [0|3] "" XXX

+BO_ 1362 GAS_SENSOR: 6 INTERCEPTOR
+ SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0507968128,-22.85856576) [0|1] "" EON
+ SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.1015936256,-22.85856576) [0|1] "" EON
+ SG_ STATE : 35|4@0+ (1,0) [0|15] "" EON
+ SG_ IDX : 39|4@0+ (1,0) [0|15] "" EON
+ SG_ CHECKSUM : 47|8@0+ (1,0) [0|255] "" EON
+
+VAL_ 1362 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ;
+
 VAL_ 3 StW_Angl 16383 "SNA" ;
 VAL_ 3 StW_AnglSens_Id 2 "MUST" 0 "PSBL" 1 "SELF" ;
 VAL_ 3 StW_AnglSens_Stat 2 "ERR" 3 "ERR_INI" 1 "INI" 0 "OK" ;
@@ -898,4 +907,3 @@ VAL_ 1160 DAS_steeringAngleRequest 16384 "ZERO_ANGLE" ;
 VAL_ 1160 DAS_steeringControlType 1 "ANGLE_CONTROL" 3 "DISABLED" 0 "NONE" 2 "RESERVED" ;
 VAL_ 1160 DAS_steeringHapticRequest 1 "ACTIVE" 0 "IDLE" ;

-
@@ -33,6 +33,7 @@
 #define SAFETY_PSA 31U
 #define SAFETY_RIVIAN 33U
 #define SAFETY_VOLKSWAGEN_MEB 34U
+#define SAFETY_TESLA_PREAP 35U

 #define GET_BIT(msg, b) ((bool)!!(((msg)->data[((b) / 8U)] >> ((b) % 8U)) & 0x1U))
 #define GET_FLAG(value, mask) (((value) & (mask)) == (mask))
@@ -215,6 +216,7 @@ typedef bool (*fwd_hook)(int bus_num, int addr);      // returns true if the mes
 typedef struct {
  safety_hook_init init;
  rx_hook rx;
+  rx_hook rx_all;
  tx_hook tx;
  fwd_hook fwd;
  get_checksum_t get_checksum;
@@ -355,3 +357,4 @@ extern const safety_hooks volkswagen_mqb_hooks;
 extern const safety_hooks volkswagen_pq_hooks;
 extern const safety_hooks rivian_hooks;
 extern const safety_hooks psa_hooks;
+extern const safety_hooks tesla_preap_hooks;
@@ -11,6 +11,7 @@
 #include "opendbc/safety/modes/honda.h"
 #include "opendbc/safety/modes/toyota.h"
 #include "opendbc/safety/modes/tesla.h"
+#include "opendbc/safety/modes/tesla_preap.h"
 #include "opendbc/safety/modes/gm.h"
 #include "opendbc/safety/modes/ford.h"
 #include "opendbc/safety/modes/hyundai.h"
@@ -198,6 +199,10 @@ static bool rx_msg_safety_check(const CANPacket_t *msg,
 bool safety_rx_hook(const CANPacket_t *msg) {
  bool controls_allowed_prev = controls_allowed;

+  if (current_hooks->rx_all != NULL) {
+    current_hooks->rx_all(msg);
+  }
+
  bool valid = rx_msg_safety_check(msg, &current_safety_config, current_hooks);
  bool whitelisted = get_addr_check_index(msg, current_safety_config.rx_checks, current_safety_config.rx_checks_len) != -1;
  bool gm_rx_passthrough = current_safety_mode == SAFETY_GM;
@@ -417,6 +422,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
    {SAFETY_FORD, &ford_hooks},
    {SAFETY_RIVIAN, &rivian_hooks},
    {SAFETY_TESLA, &tesla_hooks},
+    {SAFETY_TESLA_PREAP, &tesla_preap_hooks},
 #ifdef CANFD
    {SAFETY_HYUNDAI_CANFD, &hyundai_canfd_hooks},
 #endif
@@ -39,9 +39,9 @@ class TestTeslaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest,

  # Tesla uses get_max_angle_delta_vm and get_max_angle_vm for real lateral accel and jerk limits
  # TODO: integrate this into AngleSteeringSafetyTest
-  ANGLE_RATE_BP = None
-  ANGLE_RATE_UP = None
-  ANGLE_RATE_DOWN = None
+  ANGLE_RATE_BP = [0.]
+  ANGLE_RATE_UP = [CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE]
+  ANGLE_RATE_DOWN = [CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE]

  # Real time limits
  LATERAL_FREQUENCY = 50  # Hz
@@ -69,7 +69,9 @@ class TestTeslaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest,

    self.steer_control_types = {d: v for v, d in self.define.dv["DAS_steeringControl"]["DAS_steeringControlType"].items()}

-  def _angle_cmd_msg(self, angle: float, state: bool | int, increment_timer: bool = True, bus: int = 0):
+  def _angle_cmd_msg(self, angle: float, state: bool | int = True, increment_timer: bool = True, bus: int = 0, enabled: bool | None = None):
+    if enabled is not None:
+      state = enabled
    values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": state}
    if increment_timer:
      self.safety.set_timer(self.cnt_angle_cmd * int(1e6 / self.LATERAL_FREQUENCY))
@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-95fa880b-DEBUG";
+const uint8_t gitversion[19] = "DEV-7e481891-DEBUG";
@@ -1 +1 @@
-DEV-95fa880b-DEBUG
+DEV-7e481891-DEBUG
@@ -184,6 +184,9 @@ class CarSpecificEvents:
  def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears: list | None = None, pcm_enable=True,
                           allow_button_cancel=True, suppress_low_speed_alert=False):
    events = Events()
+    preap_software_cruise = (self.CP.brand == "tesla" and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP" and
+                             self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise)
+    pcm_enable = pcm_enable or preap_software_cruise

    if CS.doorOpen:
      events.add(EventName.doorOpen)
@@ -220,13 +220,23 @@ class Car:
    if can_rcv_valid and REPLAY:
      self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime

-    self.v_cruise_helper.update_v_cruise(
-      CS,
-      self.sm['carControl'].enabled,
-      self.is_metric,
-      self.sm['starpilotPlan'].speedLimitChanged,
-      self.starpilot_toggles,
+    preap_software_cruise = (
+      self.CP.brand == "tesla" and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP" and
+      self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise
    )
+    if not preap_software_cruise:
+      self.v_cruise_helper.update_v_cruise(
+        CS,
+        self.sm['carControl'].enabled,
+        self.is_metric,
+        self.sm['starpilotPlan'].speedLimitChanged,
+        self.starpilot_toggles,
+      )
+    else:
+      preap_v_cruise_kph = float(CS.cruiseState.speed * CV.MS_TO_KPH)
+      self.v_cruise_helper.v_cruise_kph_last = self.v_cruise_helper.v_cruise_kph
+      self.v_cruise_helper.v_cruise_kph = preap_v_cruise_kph
+      self.v_cruise_helper.v_cruise_cluster_kph = preap_v_cruise_kph
    slc_force_speed = self.params_memory.get_float("SLCForceCruiseSpeed")
    if slc_force_speed > 0:
      if self.is_metric:
@@ -237,7 +247,7 @@ class Car:
      self.v_cruise_helper.v_cruise_cluster_kph = self.v_cruise_helper.v_cruise_kph
      self.params_memory.remove("SLCForceCruiseSpeed")

-    if self.sm['carControl'].enabled and not self.CC_prev.enabled:
+    if self.sm['carControl'].enabled and not self.CC_prev.enabled and not preap_software_cruise:
      # Use CarState w/ buttons from the step selfdrived enables on
      desired_speed_limit = self.sm['starpilotPlan'].slcSpeedLimit + self.sm['starpilotPlan'].slcSpeedLimitOffset
      self.v_cruise_helper.initialize_v_cruise(
@@ -64,6 +64,16 @@ CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.02
 CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.26
 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40
+CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.04
+CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.10
+CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.02
+CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.08
+CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 0.16
+CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 0.26
+CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.01
+CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.03
+CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.08
+CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.16

 BOLT_2022_2023_CARS = (
  GM_CAR.CHEVROLET_BOLT_ACC_2022_2023,
@@ -375,15 +385,30 @@ def get_civic_bosch_modified_b_ff_scale(desired_lateral_accel: float, desired_la
  turn_in_weight = max(phase, 0.0)
  unwind_weight = max(-phase, 0.0)
  low_speed_factor = _civic_bosch_modified_b_low_speed_factor(v_ego)
+  variant_active = civic_bosch_modified_lateral_testing_ground_active()
+  if variant_active:
+    base_reduction += (_civic_bosch_modified_b_side_value(desired_lateral_accel,
+                                                           CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT,
+                                                           CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT) * onset * cutoff)

  turn_in_boost = 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel,
                                                             CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT,
                                                             CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT) *
                          turn_in_weight * (0.40 + 0.60 * low_speed_factor))
+  if variant_active:
+    turn_in_boost *= 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel,
+                                                                CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT,
+                                                                CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT) *
+                             turn_in_weight * (0.40 + 0.60 * low_speed_factor))
  unwind_taper = 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel,
                                                            CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT,
                                                            CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT) *
                         unwind_weight * (0.35 + 0.65 * low_speed_factor))
+  if variant_active:
+    unwind_taper *= 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel,
+                                                               CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT,
+                                                               CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT) *
+                            unwind_weight * (0.35 + 0.65 * low_speed_factor))
  return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)


@@ -398,16 +423,27 @@ def get_civic_bosch_modified_b_friction_scale(v_ego: float, desired_lateral_acce
  phase = _civic_bosch_modified_b_transition_phase(desired_lateral_accel, desired_lateral_jerk)
  turn_in_weight = max(phase, 0.0)
  unwind_weight = max(-phase, 0.0)
+  variant_active = civic_bosch_modified_lateral_testing_ground_active()

  friction_scale = 1.0
  friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel,
                                                         CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT,
                                                         CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT) *
                     envelope * turn_in_weight)
+  if variant_active:
+    friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel,
+                                                           CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT,
+                                                           CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT) *
+                       envelope * turn_in_weight)
  friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel,
                                                         CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT,
                                                         CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT) *
                     envelope * unwind_weight)
+  if variant_active:
+    friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel,
+                                                           CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT,
+                                                           CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT) *
+                       envelope * unwind_weight)
  return min(max(friction_scale, 0.82), 1.06)


@@ -108,6 +108,22 @@ UNCERT_MAG_TRIG = 0.50
 _A_TOTAL_MAX_V = [3.5, 3.5, 3.2]
 _A_TOTAL_MAX_BP = [0., 20., 40.]

+_preap_follow_cache = None
+
+
+def get_preap_follow_limit(v_ego):
+  global _preap_follow_cache
+  if _preap_follow_cache is None:
+    try:
+      from opendbc.car.tesla.preap.constants import ACCEL_PREAP_BP, ACCEL_PREAP_FOLLOW
+      _preap_follow_cache = (ACCEL_PREAP_BP, ACCEL_PREAP_FOLLOW)
+    except ImportError:
+      _preap_follow_cache = (None, None)
+  bp, values = _preap_follow_cache
+  if bp is None:
+    return None
+  return float(np.interp(v_ego, bp, values))
+

 def get_longitudinal_personality(sm):
  return sm['selfdriveState'].personality
@@ -220,6 +236,13 @@ class LongitudinalPlanner:
    self.model_allow_throttle = True
    self.allow_throttle = True
    self.mode = 'acc'
+    self.is_preap = (
+      CP.brand == "tesla" and CP.carFingerprint == "TESLA_MODEL_S_PREAP" and
+      CP.openpilotLongitudinalControl and not CP.pcmCruise
+    )
+    self.nap_adaptive_accel = False
+    self._preap_params = None
+    self._preap_param_frame = 0

    self.generation = None

@@ -251,6 +274,15 @@ class LongitudinalPlanner:
    self.effective_t_follow = None
    self.vision_low_speed_stop_hold_until = 0.0

+    if self.is_preap:
+      try:
+        from openpilot.common.params import Params
+        self._preap_params = Params()
+        self.nap_adaptive_accel = self._preap_params.get_bool("NAPAdaptiveAccel")
+      except Exception:
+        self._preap_params = None
+        self.nap_adaptive_accel = False
+
  @property
  def mlsim(self):
    return self.generation in ("v8", "v10", "v11", "v12", "v13")
@@ -580,6 +612,11 @@ class LongitudinalPlanner:
    return d_rel < dynamic_distance and (ttc < RAW_LEAD_SAFETY_TTC or lead_braking)

  def update(self, sm, starpilot_toggles):
+    if self.is_preap:
+      self._preap_param_frame += 1
+      if self._preap_params is not None and (self._preap_param_frame % 20) == 0:
+        self.nap_adaptive_accel = self._preap_params.get_bool("NAPAdaptiveAccel")
+
    self.generation = getattr(starpilot_toggles, "model_version", None)
    self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
    self.mpc.mode = 'acc'
@@ -661,6 +698,18 @@ class LongitudinalPlanner:
    lead_one_active = bool(self.lead_one.status and lead_control_active)
    effective_t_follow = self.get_dynamic_t_follow(sm['starpilotPlan'].tFollow, self.lead_one if lead_one_active else None, v_ego)

+    if self.is_preap and self.nap_adaptive_accel and lead_one_active:
+      follow_limit = get_preap_follow_limit(v_ego)
+      if follow_limit is not None:
+        safe_dist = get_safe_obstacle_distance(v_ego, effective_t_follow)
+        lead_dist_ratio = float(self.lead_one.dRel) / max(safe_dist, 1.0)
+        cap_strength = float(np.clip(1.0 - (lead_dist_ratio - 1.2) / 0.3, 0.0, 1.0))
+        if cap_strength > 0.0:
+          accel_limits_turns[1] = min(
+            accel_limits_turns[1],
+            accel_limits_turns[1] * (1.0 - cap_strength) + follow_limit * cap_strength,
+          )
+
    lead_dist = self.lead_one.dRel if lead_one_active else 50.0

    # Smooth lead distance (EMA) to avoid chatter in thresholds
@@ -456,6 +456,24 @@ class TestLatControl:
    assert unwind_left < 1.0
    assert unwind_right < unwind_left

+  def test_modified_civic_b_variant_extra_torque_shaping_curve(self, monkeypatch):
+    base_steady_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.0, 12.0)
+    base_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0)
+    base_unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0)
+    base_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8)
+
+    monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_lateral_testing_ground_active", lambda: True)
+
+    variant_steady_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.0, 12.0)
+    variant_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0)
+    variant_unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0)
+    variant_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8)
+
+    assert variant_steady_right < base_steady_right
+    assert variant_turn_in_right >= base_turn_in_right
+    assert variant_unwind_right < base_unwind_right
+    assert variant_unwind_right_friction < base_unwind_right_friction
+
  def test_kia_ev6_testing_ground_update_path(self, monkeypatch):
    controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_EV6)
    monkeypatch.setattr(latcontrol_torque, "kia_ev6_lateral_testing_ground_active", lambda: True)
@@ -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_760253992355356602) {
-   out_760253992355356602[0] = delta_x[0] + nom_x[0];
-   out_760253992355356602[1] = delta_x[1] + nom_x[1];
-   out_760253992355356602[2] = delta_x[2] + nom_x[2];
-   out_760253992355356602[3] = delta_x[3] + nom_x[3];
-   out_760253992355356602[4] = delta_x[4] + nom_x[4];
-   out_760253992355356602[5] = delta_x[5] + nom_x[5];
-   out_760253992355356602[6] = delta_x[6] + nom_x[6];
-   out_760253992355356602[7] = delta_x[7] + nom_x[7];
-   out_760253992355356602[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_1697783337874095353) {
+   out_1697783337874095353[0] = delta_x[0] + nom_x[0];
+   out_1697783337874095353[1] = delta_x[1] + nom_x[1];
+   out_1697783337874095353[2] = delta_x[2] + nom_x[2];
+   out_1697783337874095353[3] = delta_x[3] + nom_x[3];
+   out_1697783337874095353[4] = delta_x[4] + nom_x[4];
+   out_1697783337874095353[5] = delta_x[5] + nom_x[5];
+   out_1697783337874095353[6] = delta_x[6] + nom_x[6];
+   out_1697783337874095353[7] = delta_x[7] + nom_x[7];
+   out_1697783337874095353[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_4983548601721046404) {
-   out_4983548601721046404[0] = -nom_x[0] + true_x[0];
-   out_4983548601721046404[1] = -nom_x[1] + true_x[1];
-   out_4983548601721046404[2] = -nom_x[2] + true_x[2];
-   out_4983548601721046404[3] = -nom_x[3] + true_x[3];
-   out_4983548601721046404[4] = -nom_x[4] + true_x[4];
-   out_4983548601721046404[5] = -nom_x[5] + true_x[5];
-   out_4983548601721046404[6] = -nom_x[6] + true_x[6];
-   out_4983548601721046404[7] = -nom_x[7] + true_x[7];
-   out_4983548601721046404[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_3236445373879696441) {
+   out_3236445373879696441[0] = -nom_x[0] + true_x[0];
+   out_3236445373879696441[1] = -nom_x[1] + true_x[1];
+   out_3236445373879696441[2] = -nom_x[2] + true_x[2];
+   out_3236445373879696441[3] = -nom_x[3] + true_x[3];
+   out_3236445373879696441[4] = -nom_x[4] + true_x[4];
+   out_3236445373879696441[5] = -nom_x[5] + true_x[5];
+   out_3236445373879696441[6] = -nom_x[6] + true_x[6];
+   out_3236445373879696441[7] = -nom_x[7] + true_x[7];
+   out_3236445373879696441[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_8995833010704521871) {
-   out_8995833010704521871[0] = 1.0;
-   out_8995833010704521871[1] = 0.0;
-   out_8995833010704521871[2] = 0.0;
-   out_8995833010704521871[3] = 0.0;
-   out_8995833010704521871[4] = 0.0;
-   out_8995833010704521871[5] = 0.0;
-   out_8995833010704521871[6] = 0.0;
-   out_8995833010704521871[7] = 0.0;
-   out_8995833010704521871[8] = 0.0;
-   out_8995833010704521871[9] = 0.0;
-   out_8995833010704521871[10] = 1.0;
-   out_8995833010704521871[11] = 0.0;
-   out_8995833010704521871[12] = 0.0;
-   out_8995833010704521871[13] = 0.0;
-   out_8995833010704521871[14] = 0.0;
-   out_8995833010704521871[15] = 0.0;
-   out_8995833010704521871[16] = 0.0;
-   out_8995833010704521871[17] = 0.0;
-   out_8995833010704521871[18] = 0.0;
-   out_8995833010704521871[19] = 0.0;
-   out_8995833010704521871[20] = 1.0;
-   out_8995833010704521871[21] = 0.0;
-   out_8995833010704521871[22] = 0.0;
-   out_8995833010704521871[23] = 0.0;
-   out_8995833010704521871[24] = 0.0;
-   out_8995833010704521871[25] = 0.0;
-   out_8995833010704521871[26] = 0.0;
-   out_8995833010704521871[27] = 0.0;
-   out_8995833010704521871[28] = 0.0;
-   out_8995833010704521871[29] = 0.0;
-   out_8995833010704521871[30] = 1.0;
-   out_8995833010704521871[31] = 0.0;
-   out_8995833010704521871[32] = 0.0;
-   out_8995833010704521871[33] = 0.0;
-   out_8995833010704521871[34] = 0.0;
-   out_8995833010704521871[35] = 0.0;
-   out_8995833010704521871[36] = 0.0;
-   out_8995833010704521871[37] = 0.0;
-   out_8995833010704521871[38] = 0.0;
-   out_8995833010704521871[39] = 0.0;
-   out_8995833010704521871[40] = 1.0;
-   out_8995833010704521871[41] = 0.0;
-   out_8995833010704521871[42] = 0.0;
-   out_8995833010704521871[43] = 0.0;
-   out_8995833010704521871[44] = 0.0;
-   out_8995833010704521871[45] = 0.0;
-   out_8995833010704521871[46] = 0.0;
-   out_8995833010704521871[47] = 0.0;
-   out_8995833010704521871[48] = 0.0;
-   out_8995833010704521871[49] = 0.0;
-   out_8995833010704521871[50] = 1.0;
-   out_8995833010704521871[51] = 0.0;
-   out_8995833010704521871[52] = 0.0;
-   out_8995833010704521871[53] = 0.0;
-   out_8995833010704521871[54] = 0.0;
-   out_8995833010704521871[55] = 0.0;
-   out_8995833010704521871[56] = 0.0;
-   out_8995833010704521871[57] = 0.0;
-   out_8995833010704521871[58] = 0.0;
-   out_8995833010704521871[59] = 0.0;
-   out_8995833010704521871[60] = 1.0;
-   out_8995833010704521871[61] = 0.0;
-   out_8995833010704521871[62] = 0.0;
-   out_8995833010704521871[63] = 0.0;
-   out_8995833010704521871[64] = 0.0;
-   out_8995833010704521871[65] = 0.0;
-   out_8995833010704521871[66] = 0.0;
-   out_8995833010704521871[67] = 0.0;
-   out_8995833010704521871[68] = 0.0;
-   out_8995833010704521871[69] = 0.0;
-   out_8995833010704521871[70] = 1.0;
-   out_8995833010704521871[71] = 0.0;
-   out_8995833010704521871[72] = 0.0;
-   out_8995833010704521871[73] = 0.0;
-   out_8995833010704521871[74] = 0.0;
-   out_8995833010704521871[75] = 0.0;
-   out_8995833010704521871[76] = 0.0;
-   out_8995833010704521871[77] = 0.0;
-   out_8995833010704521871[78] = 0.0;
-   out_8995833010704521871[79] = 0.0;
-   out_8995833010704521871[80] = 1.0;
+void H_mod_fun(double *state, double *out_4178247856660158459) {
+   out_4178247856660158459[0] = 1.0;
+   out_4178247856660158459[1] = 0.0;
+   out_4178247856660158459[2] = 0.0;
+   out_4178247856660158459[3] = 0.0;
+   out_4178247856660158459[4] = 0.0;
+   out_4178247856660158459[5] = 0.0;
+   out_4178247856660158459[6] = 0.0;
+   out_4178247856660158459[7] = 0.0;
+   out_4178247856660158459[8] = 0.0;
+   out_4178247856660158459[9] = 0.0;
+   out_4178247856660158459[10] = 1.0;
+   out_4178247856660158459[11] = 0.0;
+   out_4178247856660158459[12] = 0.0;
+   out_4178247856660158459[13] = 0.0;
+   out_4178247856660158459[14] = 0.0;
+   out_4178247856660158459[15] = 0.0;
+   out_4178247856660158459[16] = 0.0;
+   out_4178247856660158459[17] = 0.0;
+   out_4178247856660158459[18] = 0.0;
+   out_4178247856660158459[19] = 0.0;
+   out_4178247856660158459[20] = 1.0;
+   out_4178247856660158459[21] = 0.0;
+   out_4178247856660158459[22] = 0.0;
+   out_4178247856660158459[23] = 0.0;
+   out_4178247856660158459[24] = 0.0;
+   out_4178247856660158459[25] = 0.0;
+   out_4178247856660158459[26] = 0.0;
+   out_4178247856660158459[27] = 0.0;
+   out_4178247856660158459[28] = 0.0;
+   out_4178247856660158459[29] = 0.0;
+   out_4178247856660158459[30] = 1.0;
+   out_4178247856660158459[31] = 0.0;
+   out_4178247856660158459[32] = 0.0;
+   out_4178247856660158459[33] = 0.0;
+   out_4178247856660158459[34] = 0.0;
+   out_4178247856660158459[35] = 0.0;
+   out_4178247856660158459[36] = 0.0;
+   out_4178247856660158459[37] = 0.0;
+   out_4178247856660158459[38] = 0.0;
+   out_4178247856660158459[39] = 0.0;
+   out_4178247856660158459[40] = 1.0;
+   out_4178247856660158459[41] = 0.0;
+   out_4178247856660158459[42] = 0.0;
+   out_4178247856660158459[43] = 0.0;
+   out_4178247856660158459[44] = 0.0;
+   out_4178247856660158459[45] = 0.0;
+   out_4178247856660158459[46] = 0.0;
+   out_4178247856660158459[47] = 0.0;
+   out_4178247856660158459[48] = 0.0;
+   out_4178247856660158459[49] = 0.0;
+   out_4178247856660158459[50] = 1.0;
+   out_4178247856660158459[51] = 0.0;
+   out_4178247856660158459[52] = 0.0;
+   out_4178247856660158459[53] = 0.0;
+   out_4178247856660158459[54] = 0.0;
+   out_4178247856660158459[55] = 0.0;
+   out_4178247856660158459[56] = 0.0;
+   out_4178247856660158459[57] = 0.0;
+   out_4178247856660158459[58] = 0.0;
+   out_4178247856660158459[59] = 0.0;
+   out_4178247856660158459[60] = 1.0;
+   out_4178247856660158459[61] = 0.0;
+   out_4178247856660158459[62] = 0.0;
+   out_4178247856660158459[63] = 0.0;
+   out_4178247856660158459[64] = 0.0;
+   out_4178247856660158459[65] = 0.0;
+   out_4178247856660158459[66] = 0.0;
+   out_4178247856660158459[67] = 0.0;
+   out_4178247856660158459[68] = 0.0;
+   out_4178247856660158459[69] = 0.0;
+   out_4178247856660158459[70] = 1.0;
+   out_4178247856660158459[71] = 0.0;
+   out_4178247856660158459[72] = 0.0;
+   out_4178247856660158459[73] = 0.0;
+   out_4178247856660158459[74] = 0.0;
+   out_4178247856660158459[75] = 0.0;
+   out_4178247856660158459[76] = 0.0;
+   out_4178247856660158459[77] = 0.0;
+   out_4178247856660158459[78] = 0.0;
+   out_4178247856660158459[79] = 0.0;
+   out_4178247856660158459[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_4562655195590537984) {
-   out_4562655195590537984[0] = state[0];
-   out_4562655195590537984[1] = state[1];
-   out_4562655195590537984[2] = state[2];
-   out_4562655195590537984[3] = state[3];
-   out_4562655195590537984[4] = state[4];
-   out_4562655195590537984[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_4562655195590537984[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_4562655195590537984[7] = state[7];
-   out_4562655195590537984[8] = state[8];
+void f_fun(double *state, double dt, double *out_6661857716321963649) {
+   out_6661857716321963649[0] = state[0];
+   out_6661857716321963649[1] = state[1];
+   out_6661857716321963649[2] = state[2];
+   out_6661857716321963649[3] = state[3];
+   out_6661857716321963649[4] = state[4];
+   out_6661857716321963649[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_6661857716321963649[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_6661857716321963649[7] = state[7];
+   out_6661857716321963649[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_6423431565507769528) {
-   out_6423431565507769528[0] = 1;
-   out_6423431565507769528[1] = 0;
-   out_6423431565507769528[2] = 0;
-   out_6423431565507769528[3] = 0;
-   out_6423431565507769528[4] = 0;
-   out_6423431565507769528[5] = 0;
-   out_6423431565507769528[6] = 0;
-   out_6423431565507769528[7] = 0;
-   out_6423431565507769528[8] = 0;
-   out_6423431565507769528[9] = 0;
-   out_6423431565507769528[10] = 1;
-   out_6423431565507769528[11] = 0;
-   out_6423431565507769528[12] = 0;
-   out_6423431565507769528[13] = 0;
-   out_6423431565507769528[14] = 0;
-   out_6423431565507769528[15] = 0;
-   out_6423431565507769528[16] = 0;
-   out_6423431565507769528[17] = 0;
-   out_6423431565507769528[18] = 0;
-   out_6423431565507769528[19] = 0;
-   out_6423431565507769528[20] = 1;
-   out_6423431565507769528[21] = 0;
-   out_6423431565507769528[22] = 0;
-   out_6423431565507769528[23] = 0;
-   out_6423431565507769528[24] = 0;
-   out_6423431565507769528[25] = 0;
-   out_6423431565507769528[26] = 0;
-   out_6423431565507769528[27] = 0;
-   out_6423431565507769528[28] = 0;
-   out_6423431565507769528[29] = 0;
-   out_6423431565507769528[30] = 1;
-   out_6423431565507769528[31] = 0;
-   out_6423431565507769528[32] = 0;
-   out_6423431565507769528[33] = 0;
-   out_6423431565507769528[34] = 0;
-   out_6423431565507769528[35] = 0;
-   out_6423431565507769528[36] = 0;
-   out_6423431565507769528[37] = 0;
-   out_6423431565507769528[38] = 0;
-   out_6423431565507769528[39] = 0;
-   out_6423431565507769528[40] = 1;
-   out_6423431565507769528[41] = 0;
-   out_6423431565507769528[42] = 0;
-   out_6423431565507769528[43] = 0;
-   out_6423431565507769528[44] = 0;
-   out_6423431565507769528[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_6423431565507769528[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_6423431565507769528[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_6423431565507769528[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_6423431565507769528[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_6423431565507769528[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_6423431565507769528[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_6423431565507769528[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_6423431565507769528[53] = -9.8100000000000005*dt;
-   out_6423431565507769528[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_6423431565507769528[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_6423431565507769528[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_6423431565507769528[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_6423431565507769528[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_6423431565507769528[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_6423431565507769528[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_6423431565507769528[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_6423431565507769528[62] = 0;
-   out_6423431565507769528[63] = 0;
-   out_6423431565507769528[64] = 0;
-   out_6423431565507769528[65] = 0;
-   out_6423431565507769528[66] = 0;
-   out_6423431565507769528[67] = 0;
-   out_6423431565507769528[68] = 0;
-   out_6423431565507769528[69] = 0;
-   out_6423431565507769528[70] = 1;
-   out_6423431565507769528[71] = 0;
-   out_6423431565507769528[72] = 0;
-   out_6423431565507769528[73] = 0;
-   out_6423431565507769528[74] = 0;
-   out_6423431565507769528[75] = 0;
-   out_6423431565507769528[76] = 0;
-   out_6423431565507769528[77] = 0;
-   out_6423431565507769528[78] = 0;
-   out_6423431565507769528[79] = 0;
-   out_6423431565507769528[80] = 1;
+void F_fun(double *state, double dt, double *out_4395736809983768071) {
+   out_4395736809983768071[0] = 1;
+   out_4395736809983768071[1] = 0;
+   out_4395736809983768071[2] = 0;
+   out_4395736809983768071[3] = 0;
+   out_4395736809983768071[4] = 0;
+   out_4395736809983768071[5] = 0;
+   out_4395736809983768071[6] = 0;
+   out_4395736809983768071[7] = 0;
+   out_4395736809983768071[8] = 0;
+   out_4395736809983768071[9] = 0;
+   out_4395736809983768071[10] = 1;
+   out_4395736809983768071[11] = 0;
+   out_4395736809983768071[12] = 0;
+   out_4395736809983768071[13] = 0;
+   out_4395736809983768071[14] = 0;
+   out_4395736809983768071[15] = 0;
+   out_4395736809983768071[16] = 0;
+   out_4395736809983768071[17] = 0;
+   out_4395736809983768071[18] = 0;
+   out_4395736809983768071[19] = 0;
+   out_4395736809983768071[20] = 1;
+   out_4395736809983768071[21] = 0;
+   out_4395736809983768071[22] = 0;
+   out_4395736809983768071[23] = 0;
+   out_4395736809983768071[24] = 0;
+   out_4395736809983768071[25] = 0;
+   out_4395736809983768071[26] = 0;
+   out_4395736809983768071[27] = 0;
+   out_4395736809983768071[28] = 0;
+   out_4395736809983768071[29] = 0;
+   out_4395736809983768071[30] = 1;
+   out_4395736809983768071[31] = 0;
+   out_4395736809983768071[32] = 0;
+   out_4395736809983768071[33] = 0;
+   out_4395736809983768071[34] = 0;
+   out_4395736809983768071[35] = 0;
+   out_4395736809983768071[36] = 0;
+   out_4395736809983768071[37] = 0;
+   out_4395736809983768071[38] = 0;
+   out_4395736809983768071[39] = 0;
+   out_4395736809983768071[40] = 1;
+   out_4395736809983768071[41] = 0;
+   out_4395736809983768071[42] = 0;
+   out_4395736809983768071[43] = 0;
+   out_4395736809983768071[44] = 0;
+   out_4395736809983768071[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_4395736809983768071[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_4395736809983768071[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_4395736809983768071[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_4395736809983768071[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_4395736809983768071[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_4395736809983768071[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_4395736809983768071[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_4395736809983768071[53] = -9.8100000000000005*dt;
+   out_4395736809983768071[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_4395736809983768071[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_4395736809983768071[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_4395736809983768071[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_4395736809983768071[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_4395736809983768071[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_4395736809983768071[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_4395736809983768071[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_4395736809983768071[62] = 0;
+   out_4395736809983768071[63] = 0;
+   out_4395736809983768071[64] = 0;
+   out_4395736809983768071[65] = 0;
+   out_4395736809983768071[66] = 0;
+   out_4395736809983768071[67] = 0;
+   out_4395736809983768071[68] = 0;
+   out_4395736809983768071[69] = 0;
+   out_4395736809983768071[70] = 1;
+   out_4395736809983768071[71] = 0;
+   out_4395736809983768071[72] = 0;
+   out_4395736809983768071[73] = 0;
+   out_4395736809983768071[74] = 0;
+   out_4395736809983768071[75] = 0;
+   out_4395736809983768071[76] = 0;
+   out_4395736809983768071[77] = 0;
+   out_4395736809983768071[78] = 0;
+   out_4395736809983768071[79] = 0;
+   out_4395736809983768071[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_8281489855210528984) {
-   out_8281489855210528984[0] = state[6];
+void h_25(double *state, double *unused, double *out_2333658162275422577) {
+   out_2333658162275422577[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_257722837092792508) {
-   out_257722837092792508[0] = 0;
-   out_257722837092792508[1] = 0;
-   out_257722837092792508[2] = 0;
-   out_257722837092792508[3] = 0;
-   out_257722837092792508[4] = 0;
-   out_257722837092792508[5] = 0;
-   out_257722837092792508[6] = 1;
-   out_257722837092792508[7] = 0;
-   out_257722837092792508[8] = 0;
+void H_25(double *state, double *unused, double *out_725443968524506381) {
+   out_725443968524506381[0] = 0;
+   out_725443968524506381[1] = 0;
+   out_725443968524506381[2] = 0;
+   out_725443968524506381[3] = 0;
+   out_725443968524506381[4] = 0;
+   out_725443968524506381[5] = 0;
+   out_725443968524506381[6] = 1;
+   out_725443968524506381[7] = 0;
+   out_725443968524506381[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_5654195456743599601) {
-   out_5654195456743599601[0] = state[4];
-   out_5654195456743599601[1] = state[5];
+void h_24(double *state, double *unused, double *out_7377061376079036481) {
+   out_7377061376079036481[0] = state[4];
+   out_7377061376079036481[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_2483430621071661070) {
-   out_2483430621071661070[0] = 0;
-   out_2483430621071661070[1] = 0;
-   out_2483430621071661070[2] = 0;
-   out_2483430621071661070[3] = 0;
-   out_2483430621071661070[4] = 1;
-   out_2483430621071661070[5] = 0;
-   out_2483430621071661070[6] = 0;
-   out_2483430621071661070[7] = 0;
-   out_2483430621071661070[8] = 0;
-   out_2483430621071661070[9] = 0;
-   out_2483430621071661070[10] = 0;
-   out_2483430621071661070[11] = 0;
-   out_2483430621071661070[12] = 0;
-   out_2483430621071661070[13] = 0;
-   out_2483430621071661070[14] = 1;
-   out_2483430621071661070[15] = 0;
-   out_2483430621071661070[16] = 0;
-   out_2483430621071661070[17] = 0;
+void H_24(double *state, double *unused, double *out_1570440577308201780) {
+   out_1570440577308201780[0] = 0;
+   out_1570440577308201780[1] = 0;
+   out_1570440577308201780[2] = 0;
+   out_1570440577308201780[3] = 0;
+   out_1570440577308201780[4] = 1;
+   out_1570440577308201780[5] = 0;
+   out_1570440577308201780[6] = 0;
+   out_1570440577308201780[7] = 0;
+   out_1570440577308201780[8] = 0;
+   out_1570440577308201780[9] = 0;
+   out_1570440577308201780[10] = 0;
+   out_1570440577308201780[11] = 0;
+   out_1570440577308201780[12] = 0;
+   out_1570440577308201780[13] = 0;
+   out_1570440577308201780[14] = 1;
+   out_1570440577308201780[15] = 0;
+   out_1570440577308201780[16] = 0;
+   out_1570440577308201780[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_8438676523561658129) {
-   out_8438676523561658129[0] = state[4];
+void h_30(double *state, double *unused, double *out_2058464099990916688) {
+   out_2058464099990916688[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_7174413178584409263) {
-   out_7174413178584409263[0] = 0;
-   out_7174413178584409263[1] = 0;
-   out_7174413178584409263[2] = 0;
-   out_7174413178584409263[3] = 0;
-   out_7174413178584409263[4] = 1;
-   out_7174413178584409263[5] = 0;
-   out_7174413178584409263[6] = 0;
-   out_7174413178584409263[7] = 0;
-   out_7174413178584409263[8] = 0;
+void H_30(double *state, double *unused, double *out_3243776927031755008) {
+   out_3243776927031755008[0] = 0;
+   out_3243776927031755008[1] = 0;
+   out_3243776927031755008[2] = 0;
+   out_3243776927031755008[3] = 0;
+   out_3243776927031755008[4] = 1;
+   out_3243776927031755008[5] = 0;
+   out_3243776927031755008[6] = 0;
+   out_3243776927031755008[7] = 0;
+   out_3243776927031755008[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_1419904868343052079) {
-   out_1419904868343052079[0] = state[7];
+void h_26(double *state, double *unused, double *out_8738934800407300586) {
+   out_8738934800407300586[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_3562248806853593109) {
-   out_3562248806853593109[0] = 0;
-   out_3562248806853593109[1] = 0;
-   out_3562248806853593109[2] = 0;
-   out_3562248806853593109[3] = 0;
-   out_3562248806853593109[4] = 0;
-   out_3562248806853593109[5] = 0;
-   out_3562248806853593109[6] = 0;
-   out_3562248806853593109[7] = 1;
-   out_3562248806853593109[8] = 0;
+void H_26(double *state, double *unused, double *out_3016059350349549843) {
+   out_3016059350349549843[0] = 0;
+   out_3016059350349549843[1] = 0;
+   out_3016059350349549843[2] = 0;
+   out_3016059350349549843[3] = 0;
+   out_3016059350349549843[4] = 0;
+   out_3016059350349549843[5] = 0;
+   out_3016059350349549843[6] = 0;
+   out_3016059350349549843[7] = 1;
+   out_3016059350349549843[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_1905364559686504598) {
-   out_1905364559686504598[0] = state[3];
+void h_27(double *state, double *unused, double *out_3277168015177085012) {
+   out_3277168015177085012[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_4999649866783984352) {
-   out_4999649866783984352[0] = 0;
-   out_4999649866783984352[1] = 0;
-   out_4999649866783984352[2] = 0;
-   out_4999649866783984352[3] = 1;
-   out_4999649866783984352[4] = 0;
-   out_4999649866783984352[5] = 0;
-   out_4999649866783984352[6] = 0;
-   out_4999649866783984352[7] = 0;
-   out_4999649866783984352[8] = 0;
+void H_27(double *state, double *unused, double *out_5467370998215698225) {
+   out_5467370998215698225[0] = 0;
+   out_5467370998215698225[1] = 0;
+   out_5467370998215698225[2] = 0;
+   out_5467370998215698225[3] = 1;
+   out_5467370998215698225[4] = 0;
+   out_5467370998215698225[5] = 0;
+   out_5467370998215698225[6] = 0;
+   out_5467370998215698225[7] = 0;
+   out_5467370998215698225[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_3005304846354827708) {
-   out_3005304846354827708[0] = state[1];
+void h_29(double *state, double *unused, double *out_4679132473327541980) {
+   out_4679132473327541980[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_7684644522898801447) {
-   out_7684644522898801447[0] = 0;
-   out_7684644522898801447[1] = 1;
-   out_7684644522898801447[2] = 0;
-   out_7684644522898801447[3] = 0;
-   out_7684644522898801447[4] = 0;
-   out_7684644522898801447[5] = 0;
-   out_7684644522898801447[6] = 0;
-   out_7684644522898801447[7] = 0;
-   out_7684644522898801447[8] = 0;
+void H_29(double *state, double *unused, double *out_3754008271346147192) {
+   out_3754008271346147192[0] = 0;
+   out_3754008271346147192[1] = 1;
+   out_3754008271346147192[2] = 0;
+   out_3754008271346147192[3] = 0;
+   out_3754008271346147192[4] = 0;
+   out_3754008271346147192[5] = 0;
+   out_3754008271346147192[6] = 0;
+   out_3754008271346147192[7] = 0;
+   out_3754008271346147192[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_8832630219803516209) {
-   out_8832630219803516209[0] = state[0];
+void h_28(double *state, double *unused, double *out_1724505252483760633) {
+   out_1724505252483760633[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_1796111877155097255) {
-   out_1796111877155097255[0] = 1;
-   out_1796111877155097255[1] = 0;
-   out_1796111877155097255[2] = 0;
-   out_1796111877155097255[3] = 0;
-   out_1796111877155097255[4] = 0;
-   out_1796111877155097255[5] = 0;
-   out_1796111877155097255[6] = 0;
-   out_1796111877155097255[7] = 0;
-   out_1796111877155097255[8] = 0;
+void H_28(double *state, double *unused, double *out_1328390745723383382) {
+   out_1328390745723383382[0] = 1;
+   out_1328390745723383382[1] = 0;
+   out_1328390745723383382[2] = 0;
+   out_1328390745723383382[3] = 0;
+   out_1328390745723383382[4] = 0;
+   out_1328390745723383382[5] = 0;
+   out_1328390745723383382[6] = 0;
+   out_1328390745723383382[7] = 0;
+   out_1328390745723383382[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_6225098197673216166) {
-   out_6225098197673216166[0] = state[8];
+void h_31(double *state, double *unused, double *out_6530453262617598609) {
+   out_6530453262617598609[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_7334398087604609761) {
-   out_7334398087604609761[0] = 0;
-   out_7334398087604609761[1] = 0;
-   out_7334398087604609761[2] = 0;
-   out_7334398087604609761[3] = 0;
-   out_7334398087604609761[4] = 0;
-   out_7334398087604609761[5] = 0;
-   out_7334398087604609761[6] = 0;
-   out_7334398087604609761[7] = 0;
-   out_7334398087604609761[8] = 1;
+void H_31(double *state, double *unused, double *out_756089930401466809) {
+   out_756089930401466809[0] = 0;
+   out_756089930401466809[1] = 0;
+   out_756089930401466809[2] = 0;
+   out_756089930401466809[3] = 0;
+   out_756089930401466809[4] = 0;
+   out_756089930401466809[5] = 0;
+   out_756089930401466809[6] = 0;
+   out_756089930401466809[7] = 0;
+   out_756089930401466809[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_760253992355356602) {
-  err_fun(nom_x, delta_x, out_760253992355356602);
+void car_err_fun(double *nom_x, double *delta_x, double *out_1697783337874095353) {
+  err_fun(nom_x, delta_x, out_1697783337874095353);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_4983548601721046404) {
-  inv_err_fun(nom_x, true_x, out_4983548601721046404);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_3236445373879696441) {
+  inv_err_fun(nom_x, true_x, out_3236445373879696441);
 }
-void car_H_mod_fun(double *state, double *out_8995833010704521871) {
-  H_mod_fun(state, out_8995833010704521871);
+void car_H_mod_fun(double *state, double *out_4178247856660158459) {
+  H_mod_fun(state, out_4178247856660158459);
 }
-void car_f_fun(double *state, double dt, double *out_4562655195590537984) {
-  f_fun(state,  dt, out_4562655195590537984);
+void car_f_fun(double *state, double dt, double *out_6661857716321963649) {
+  f_fun(state,  dt, out_6661857716321963649);
 }
-void car_F_fun(double *state, double dt, double *out_6423431565507769528) {
-  F_fun(state,  dt, out_6423431565507769528);
+void car_F_fun(double *state, double dt, double *out_4395736809983768071) {
+  F_fun(state,  dt, out_4395736809983768071);
 }
-void car_h_25(double *state, double *unused, double *out_8281489855210528984) {
-  h_25(state, unused, out_8281489855210528984);
+void car_h_25(double *state, double *unused, double *out_2333658162275422577) {
+  h_25(state, unused, out_2333658162275422577);
 }
-void car_H_25(double *state, double *unused, double *out_257722837092792508) {
-  H_25(state, unused, out_257722837092792508);
+void car_H_25(double *state, double *unused, double *out_725443968524506381) {
+  H_25(state, unused, out_725443968524506381);
 }
-void car_h_24(double *state, double *unused, double *out_5654195456743599601) {
-  h_24(state, unused, out_5654195456743599601);
+void car_h_24(double *state, double *unused, double *out_7377061376079036481) {
+  h_24(state, unused, out_7377061376079036481);
 }
-void car_H_24(double *state, double *unused, double *out_2483430621071661070) {
-  H_24(state, unused, out_2483430621071661070);
+void car_H_24(double *state, double *unused, double *out_1570440577308201780) {
+  H_24(state, unused, out_1570440577308201780);
 }
-void car_h_30(double *state, double *unused, double *out_8438676523561658129) {
-  h_30(state, unused, out_8438676523561658129);
+void car_h_30(double *state, double *unused, double *out_2058464099990916688) {
+  h_30(state, unused, out_2058464099990916688);
 }
-void car_H_30(double *state, double *unused, double *out_7174413178584409263) {
-  H_30(state, unused, out_7174413178584409263);
+void car_H_30(double *state, double *unused, double *out_3243776927031755008) {
+  H_30(state, unused, out_3243776927031755008);
 }
-void car_h_26(double *state, double *unused, double *out_1419904868343052079) {
-  h_26(state, unused, out_1419904868343052079);
+void car_h_26(double *state, double *unused, double *out_8738934800407300586) {
+  h_26(state, unused, out_8738934800407300586);
 }
-void car_H_26(double *state, double *unused, double *out_3562248806853593109) {
-  H_26(state, unused, out_3562248806853593109);
+void car_H_26(double *state, double *unused, double *out_3016059350349549843) {
+  H_26(state, unused, out_3016059350349549843);
 }
-void car_h_27(double *state, double *unused, double *out_1905364559686504598) {
-  h_27(state, unused, out_1905364559686504598);
+void car_h_27(double *state, double *unused, double *out_3277168015177085012) {
+  h_27(state, unused, out_3277168015177085012);
 }
-void car_H_27(double *state, double *unused, double *out_4999649866783984352) {
-  H_27(state, unused, out_4999649866783984352);
+void car_H_27(double *state, double *unused, double *out_5467370998215698225) {
+  H_27(state, unused, out_5467370998215698225);
 }
-void car_h_29(double *state, double *unused, double *out_3005304846354827708) {
-  h_29(state, unused, out_3005304846354827708);
+void car_h_29(double *state, double *unused, double *out_4679132473327541980) {
+  h_29(state, unused, out_4679132473327541980);
 }
-void car_H_29(double *state, double *unused, double *out_7684644522898801447) {
-  H_29(state, unused, out_7684644522898801447);
+void car_H_29(double *state, double *unused, double *out_3754008271346147192) {
+  H_29(state, unused, out_3754008271346147192);
 }
-void car_h_28(double *state, double *unused, double *out_8832630219803516209) {
-  h_28(state, unused, out_8832630219803516209);
+void car_h_28(double *state, double *unused, double *out_1724505252483760633) {
+  h_28(state, unused, out_1724505252483760633);
 }
-void car_H_28(double *state, double *unused, double *out_1796111877155097255) {
-  H_28(state, unused, out_1796111877155097255);
+void car_H_28(double *state, double *unused, double *out_1328390745723383382) {
+  H_28(state, unused, out_1328390745723383382);
 }
-void car_h_31(double *state, double *unused, double *out_6225098197673216166) {
-  h_31(state, unused, out_6225098197673216166);
+void car_h_31(double *state, double *unused, double *out_6530453262617598609) {
+  h_31(state, unused, out_6530453262617598609);
 }
-void car_H_31(double *state, double *unused, double *out_7334398087604609761) {
-  H_31(state, unused, out_7334398087604609761);
+void car_H_31(double *state, double *unused, double *out_756089930401466809) {
+  H_31(state, unused, out_756089930401466809);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
  predict(in_x, in_P, in_Q, dt);
@@ -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_760253992355356602);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_4983548601721046404);
-void car_H_mod_fun(double *state, double *out_8995833010704521871);
-void car_f_fun(double *state, double dt, double *out_4562655195590537984);
-void car_F_fun(double *state, double dt, double *out_6423431565507769528);
-void car_h_25(double *state, double *unused, double *out_8281489855210528984);
-void car_H_25(double *state, double *unused, double *out_257722837092792508);
-void car_h_24(double *state, double *unused, double *out_5654195456743599601);
-void car_H_24(double *state, double *unused, double *out_2483430621071661070);
-void car_h_30(double *state, double *unused, double *out_8438676523561658129);
-void car_H_30(double *state, double *unused, double *out_7174413178584409263);
-void car_h_26(double *state, double *unused, double *out_1419904868343052079);
-void car_H_26(double *state, double *unused, double *out_3562248806853593109);
-void car_h_27(double *state, double *unused, double *out_1905364559686504598);
-void car_H_27(double *state, double *unused, double *out_4999649866783984352);
-void car_h_29(double *state, double *unused, double *out_3005304846354827708);
-void car_H_29(double *state, double *unused, double *out_7684644522898801447);
-void car_h_28(double *state, double *unused, double *out_8832630219803516209);
-void car_H_28(double *state, double *unused, double *out_1796111877155097255);
-void car_h_31(double *state, double *unused, double *out_6225098197673216166);
-void car_H_31(double *state, double *unused, double *out_7334398087604609761);
+void car_err_fun(double *nom_x, double *delta_x, double *out_1697783337874095353);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_3236445373879696441);
+void car_H_mod_fun(double *state, double *out_4178247856660158459);
+void car_f_fun(double *state, double dt, double *out_6661857716321963649);
+void car_F_fun(double *state, double dt, double *out_4395736809983768071);
+void car_h_25(double *state, double *unused, double *out_2333658162275422577);
+void car_H_25(double *state, double *unused, double *out_725443968524506381);
+void car_h_24(double *state, double *unused, double *out_7377061376079036481);
+void car_H_24(double *state, double *unused, double *out_1570440577308201780);
+void car_h_30(double *state, double *unused, double *out_2058464099990916688);
+void car_H_30(double *state, double *unused, double *out_3243776927031755008);
+void car_h_26(double *state, double *unused, double *out_8738934800407300586);
+void car_H_26(double *state, double *unused, double *out_3016059350349549843);
+void car_h_27(double *state, double *unused, double *out_3277168015177085012);
+void car_H_27(double *state, double *unused, double *out_5467370998215698225);
+void car_h_29(double *state, double *unused, double *out_4679132473327541980);
+void car_H_29(double *state, double *unused, double *out_3754008271346147192);
+void car_h_28(double *state, double *unused, double *out_1724505252483760633);
+void car_H_28(double *state, double *unused, double *out_1328390745723383382);
+void car_h_31(double *state, double *unused, double *out_6530453262617598609);
+void car_H_31(double *state, double *unused, double *out_756089930401466809);
 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);
@@ -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_7113141565155944071);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4273806753012627861);
-void pose_H_mod_fun(double *state, double *out_4327706851715669235);
-void pose_f_fun(double *state, double dt, double *out_497730658810754078);
-void pose_F_fun(double *state, double dt, double *out_6088738551748760158);
-void pose_h_4(double *state, double *unused, double *out_2742355909746826294);
-void pose_H_4(double *state, double *unused, double *out_3203571547522250360);
-void pose_h_10(double *state, double *unused, double *out_5392750946095808839);
-void pose_H_10(double *state, double *unused, double *out_6832887606116687430);
-void pose_h_13(double *state, double *unused, double *out_7827281880185644605);
-void pose_H_13(double *state, double *unused, double *out_8702277810082441);
-void pose_h_14(double *state, double *unused, double *out_3917682265727873259);
-void pose_H_14(double *state, double *unused, double *out_759669308817234169);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_4940663026929446750);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1207218016783825442);
+void pose_H_mod_fun(double *state, double *out_2820660951987973699);
+void pose_f_fun(double *state, double dt, double *out_2310584025056258416);
+void pose_F_fun(double *state, double dt, double *out_6557190266781458971);
+void pose_h_4(double *state, double *unused, double *out_8177424571477984765);
+void pose_H_4(double *state, double *unused, double *out_6592468161831881271);
+void pose_h_10(double *state, double *unused, double *out_4220186743788310408);
+void pose_H_10(double *state, double *unused, double *out_2494414403690571241);
+void pose_h_13(double *state, double *unused, double *out_5780074361933493057);
+void pose_H_13(double *state, double *unused, double *out_8642002086545337544);
+void pose_h_14(double *state, double *unused, double *out_3473232613379301184);
+void pose_H_14(double *state, double *unused, double *out_3509679729536508975);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }
@@ -253,11 +253,22 @@ class SelfdriveD:
          # body always wants to enable
          self.events.add(EventName.pcmEnable)

-      # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0.
-      if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
-        (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
-        (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
+      gas_disable = CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator
+      brake_or_regen_disable = (
+        (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or
+        (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill))
+      )
+      preap_steering_only_brake = (
+        self.CP.brand == "tesla" and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP" and
+        self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise
+      )
+      if gas_disable:
        self.events.add(EventName.pedalPressed)
+      elif brake_or_regen_disable:
+        if preap_steering_only_brake:
+          self.events.add(EventName.gasPressedOverride)
+        else:
+          self.events.add(EventName.pedalPressed)

    # Create events for temperature, disk space, and memory
    if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
@@ -432,7 +443,12 @@ class SelfdriveD:
        self.enabled,
        self.sm['starpilotCarState'].alwaysOnLateralEnabled,
      )
-      cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) and not pacifica_hybrid_aol
+      preap_software_cruise = (
+        self.CP.brand == "tesla" and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP" and
+        self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise
+      )
+      effective_pcm_cruise = self.CP.pcmCruise or preap_software_cruise
+      cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not effective_pcm_cruise) and not pacifica_hybrid_aol
      self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
      if self.cruise_mismatch_counter > int(6. / DT_CTRL):
        self.events.add(EventName.cruiseMismatch)
@@ -77,6 +77,13 @@ LEGACY_STARPILOT_STATS_KEY_RENAMES = {
  "FrogPilotSeconds": "StarPilotSeconds",
 }

+LEGACY_VOLT_STOCK_ACC_CARS = {
+  GM_CAR.CHEVROLET_VOLT,
+  GM_CAR.CHEVROLET_VOLT_2019,
+  GM_CAR.CHEVROLET_VOLT_ASCM,
+  GM_CAR.CHEVROLET_VOLT_CAMERA,
+}
+
 RESOURCES_REPO = os.getenv("STARPILOT_RESOURCES_REPO", "firestar5683/StarPilot-Resources")

 ACTIVE_THEME_PATH = Path(BASEDIR) / "starpilot/assets/active_theme"
@@ -1193,15 +1200,10 @@ class StarPilotVariables:
      condition=toggle.car_make == "gm" and toggle.has_pedal and "BOLT" in toggle.car_model,
    )

-    gm_auto_hold_supported = toggle.car_model in {
-      GM_CAR.CHEVROLET_VOLT,
-      GM_CAR.CHEVROLET_VOLT_2019,
-      GM_CAR.CHEVROLET_VOLT_ASCM,
-      GM_CAR.CHEVROLET_VOLT_CAMERA,
-    }
+    gm_auto_hold_supported = toggle.car_model in LEGACY_VOLT_STOCK_ACC_CARS
    toggle.gm_auto_hold = self.get_value("GMAutoHold", condition=gm_auto_hold_supported)

-    toggle.volt_sng = self.get_value("VoltSNG", condition=toggle.car_model == "CHEVROLET_VOLT")
+    toggle.volt_sng = self.get_value("VoltSNG", condition=toggle.car_model in LEGACY_VOLT_STOCK_ACC_CARS)

    process_starpilot_toggles.cache_clear()
    self.params_memory.remove("StarPilotTogglesUpdated")
@@ -3,6 +3,15 @@ from types import SimpleNamespace
 from openpilot.starpilot.common import starpilot_variables as spv


+def test_legacy_volt_stock_acc_models_share_sng_and_auto_hold_scope():
+  assert spv.LEGACY_VOLT_STOCK_ACC_CARS == {
+    "CHEVROLET_VOLT",
+    "CHEVROLET_VOLT_2019",
+    "CHEVROLET_VOLT_ASCM",
+    "CHEVROLET_VOLT_CAMERA",
+  }
+
+
 def test_get_starpilot_toggles_uses_last_non_empty_broadcast(monkeypatch):
  params = SimpleNamespace(get_bool=lambda _key: False)
  monkeypatch.setattr(spv.get_starpilot_toggles, "_params", params, raising=False)
@@ -2143,6 +2143,100 @@
        "data_type": "bool",
        "ui_type": "toggle"
      },
+      {
+        "key": "NAPRadarEnabled",
+        "label": "Tesla Pre-AP Bosch Radar",
+        "description": "Use the stock Bosch radar path on pre-Autopilot Teslas.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "is_parent_toggle": true
+      },
+      {
+        "key": "NAPRadarBehindNosecone",
+        "label": "Radar Behind Nosecone",
+        "description": "Enable this if the Bosch radar is mounted behind the nosecone instead of being exposed.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "parent_key": "NAPRadarEnabled"
+      },
+      {
+        "key": "NAPRadarOffset",
+        "label": "Radar Offset",
+        "description": "Adjust the radar position offset in meters if the radar alignment needs a small correction.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": -2.0,
+        "max": 2.0,
+        "step": 0.01,
+        "precision": 2,
+        "parent_key": "NAPRadarEnabled"
+      },
+      {
+        "key": "NAPPedalEnabled",
+        "label": "Tesla Pre-AP comma Pedal Longitudinal",
+        "description": "Use a comma pedal interceptor for acceleration and regenerative braking on pre-Autopilot Teslas.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "is_parent_toggle": true
+      },
+      {
+        "key": "NAPPedalCanBus",
+        "label": "Pedal CAN Bus",
+        "description": "Select the CAN bus used by the Tesla pedal interceptor.",
+        "data_type": "int",
+        "ui_type": "dropdown",
+        "parent_key": "NAPPedalEnabled",
+        "options": [
+          {
+            "value": 0,
+            "label": "CAN 0"
+          },
+          {
+            "value": 2,
+            "label": "CAN 2"
+          }
+        ]
+      },
+      {
+        "key": "NAPAdaptiveAccel",
+        "label": "Adaptive Acceleration Limit",
+        "description": "Reduce maximum acceleration as you close in on a lead vehicle when Tesla pedal-long is active.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "parent_key": "NAPPedalEnabled"
+      },
+      {
+        "key": "NAPPedalCalibDone",
+        "label": "Pedal Calibration Complete",
+        "description": "Enable only after entering valid Tesla pedal calibration values. Pedal-long stays inactive until this is on.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "parent_key": "NAPPedalEnabled"
+      },
+      {
+        "key": "NAPPedalCalibFactor",
+        "label": "Pedal Calibration Factor",
+        "description": "Scaling factor used to convert Tesla pedal interceptor output into the internal DI range.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": 0.01,
+        "max": 10.0,
+        "step": 0.01,
+        "precision": 2,
+        "parent_key": "NAPPedalEnabled"
+      },
+      {
+        "key": "NAPPedalCalibZero",
+        "label": "Pedal Calibration Zero",
+        "description": "Zero point used to convert Tesla pedal interceptor output into the internal DI range.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": -50.0,
+        "max": 50.0,
+        "step": 0.01,
+        "precision": 2,
+        "parent_key": "NAPPedalEnabled"
+      },
      {
        "key": "GMPedalLongitudinal",
        "label": "Use Pedal For Longitudinal",