Enhance VASM inference with dual-threshold hysteresis and confidence hold-off

Replace legacy binary detection with tri-class model. Add EMA smoothing with dual-threshold hysteresis
(activate at conf_thresh, deactivate at conf_thresh - 0.15) and update
tests and settings for the new model.

Update v_asm_model.onnx with new model weights

common/params_keys.h

@@ -276,7 +276,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DeveloperSidebarMetric7", {PERSISTENT, INT, "7", "0", 3}},
     {"DeveloperUI", {PERSISTENT, BOOL, "0", "0", 3}},
     {"GalaxyDeveloperMode", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
-    {"TestModelLeadTrajectory", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
     {"DeveloperWidgets", {PERSISTENT, BOOL, "1", "0", 3}},
     {"DeviceManagement", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
     {"DeviceShutdown", {PERSISTENT, INT, "6", "6", 1, SETTINGS_SIMPLE}},
@@ -693,7 +692,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"UserFavorites", {PERSISTENT, STRING, "", "", 1}},
     {"UseVienna", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
     {"VASMAnnotationConfig", {PERSISTENT, JSON, "{}", "{}", 2}},
-    {"VASMConfidenceThreshold", {PERSISTENT, FLOAT, "0.85", "0.85", 2}},
+    {"VASMConfidenceThreshold", {PERSISTENT, FLOAT, "0.94", "0.94", 2}},
     {"VASMEnabled", {PERSISTENT, BOOL, "0", "0", 1}},
     {"VASMLeftActive", {CLEAR_ON_MANAGER_START, STRING, "0", "0", 2}},
     {"VASMLeftConfidence", {CLEAR_ON_MANAGER_START, STRING, "0.0", "0.0", 2}},

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

@@ -706,30 +706,6 @@ class TestHyundaiFingerprint:
     assert combined_safety_param & HyundaiSafetyFlags.LONG
     assert combined_safety_param & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE
 
-  def test_sonata_hybrid_aol_main_lkas_sync_is_scoped(self):
-    toggles = SimpleNamespace(always_on_lateral_lkas=True, main_cruise_aol_toggle=True)
-
-    sonata_hybrid_cp = CarInterface.get_params(CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], False, False, False, None)
-    sonata_hybrid_fpcp = CarInterface.get_starpilot_params(
-      CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], sonata_hybrid_cp, toggles,
-    )
-    assert sonata_hybrid_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC
-
-    sonata_cp = CarInterface.get_params(CAR.HYUNDAI_SONATA, gen_empty_fingerprint(), [], False, False, False, None)
-    sonata_fpcp = CarInterface.get_starpilot_params(CAR.HYUNDAI_SONATA, gen_empty_fingerprint(), [], sonata_cp, toggles)
-    assert not (sonata_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC)
-
-    disabled_toggles = SimpleNamespace(always_on_lateral_lkas=True, main_cruise_aol_toggle=False)
-    disabled_fpcp = CarInterface.get_starpilot_params(
-      CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], sonata_hybrid_cp, disabled_toggles,
-    )
-    assert not (disabled_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC)
-
-    minimal_fpcp = CarInterface.get_starpilot_params(
-      CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], sonata_hybrid_cp, SimpleNamespace(),
-    )
-    assert not (minimal_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC)
-
   @pytest.mark.parametrize("candidate", (CAR.HYUNDAI_ELANTRA_2021, CAR.HYUNDAI_SONATA_HYBRID))
   def test_legacy_hyundai_long_does_not_gate_availability_on_main_cruise(self, candidate):
     toggles = get_test_toggles()

opendbc_repo/opendbc/car/hyundai/values.py

@@ -111,7 +111,6 @@ class HyundaiSafetyFlags(IntFlag):
 
 
 class HyundaiStarPilotSafetyFlags(IntFlag):
-  AOL_MAIN_LKAS_SYNC = 32
   HAS_LDA_BUTTON = 1024
   AOL_LKAS_ON_ENGAGE = 2048
 

opendbc_repo/opendbc/car/interfaces.py

@@ -261,9 +261,6 @@ class CarInterfaceBase(ABC):
         if params.get_bool("AlwaysOnLateral") and params.get_int("LKASButtonControl") == 9:
           fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value
 
-        if candidate == HYUNDAI.HYUNDAI_SONATA_HYBRID and getattr(starpilot_toggles, "always_on_lateral_lkas", False) and \
-            getattr(starpilot_toggles, "main_cruise_aol_toggle", False):
-          fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC.value
       elif platform in TOYOTA:
         fp_ret.canUsePedal = not CP.autoResumeSng
         fp_ret.canUseSDSU = candidate not in UNSUPPORTED_DSU_CAR and candidate not in TSS2_CAR
@@ -274,7 +271,7 @@ class CarInterfaceBase(ABC):
         if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR):
           fp_ret.flags |= ToyotaStarPilotFlags.SMART_DSU.value
 
-        if candidate == TOYOTA.TOYOTA_PRIUS:
+        if candidate in (TOYOTA.TOYOTA_PRIUS, TOYOTA.TOYOTA_PRIUS_RETROFIT):
           if 0x23 in fingerprint[0]:
             fp_ret.flags |= ToyotaStarPilotFlags.ZSS.value
 

opendbc_repo/opendbc/car/tests/test_can_fingerprint.py

@@ -27,7 +27,7 @@ class TestCanFingerprint:
       fingerprint_iter = iter([can])
       car_fingerprint, finger = can_fingerprint(lambda **kwargs: [next(fingerprint_iter, [])])  # noqa: B023
 
-      if car_model == TOYOTA.TOYOTA_MATRIX_RETROFIT:
+      if car_model in (TOYOTA.TOYOTA_MATRIX_RETROFIT, TOYOTA.TOYOTA_PRIUS_RETROFIT):
         assert fingerprint == {}
         assert car_fingerprint is None
       elif car_fingerprint is None and str(car_model).startswith(("BUICK_", "CADILLAC_", "CHEVROLET_", "GMC_", "HOLDEN_")):

opendbc_repo/opendbc/car/torque_data/substitute.toml

@@ -9,6 +9,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 
 "TOYOTA_ALPHARD_TSS2" = "TOYOTA_SIENNA"
 "TOYOTA_PRIUS_V" = "TOYOTA_PRIUS"
+"TOYOTA_PRIUS_RETROFIT" = "TOYOTA_PRIUS"
 "TOYOTA_SIENNA_4TH_GEN" = "TOYOTA_RAV4_PRIME"
 "LEXUS_IS" = "LEXUS_NX"
 "LEXUS_CTH" = "LEXUS_NX"

opendbc_repo/opendbc/car/toyota/carcontroller.py

@@ -11,7 +11,7 @@ from opendbc.car.interfaces import CarControllerBase
 from opendbc.car.toyota import toyotacan
 from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, NO_STOP_TIMER_CAR, PEDAL_TRANSITION, TSS2_CAR, \
                                         CarControllerParams, ToyotaFlags, \
-                                        UNSUPPORTED_DSU_CAR
+                                        UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR
 from opendbc.can import CANPacker
 
 Ecu = structs.CarParams.Ecu
@@ -59,13 +59,17 @@ def is_camry_hybrid(CP) -> bool:
 
 
 def is_ths_hybrid(CP) -> bool:
-  return CP.carFingerprint == CAR.TOYOTA_PRIUS or is_camry_hybrid(CP)
+  return CP.carFingerprint in LEGACY_PRIUS_CAR or is_camry_hybrid(CP)
 
 
-def should_bypass_toyota_long_pid(CP) -> bool:
+def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
+  highlander_sdsu = (
+    CP.carFingerprint == CAR.TOYOTA_HIGHLANDER and
+    bool(getattr(starpilot_toggles, "has_sdsu", False))
+  )
   return bool(CP.enableGasInterceptorDEPRECATED or (
     CP.carFingerprint == CAR.TOYOTA_CAMRY and not is_camry_hybrid(CP)
-  ))
+  ) or highlander_sdsu)
 
 
 def get_long_tune(CP, params):
@@ -74,7 +78,7 @@ def get_long_tune(CP, params):
   k_f = 1.0
 
   if is_ths_hybrid(CP):
-    k_f = 0.8 if CP.carFingerprint == CAR.TOYOTA_PRIUS else 1.0
+    k_f = 0.8 if CP.carFingerprint in LEGACY_PRIUS_CAR else 1.0
   elif CP.carFingerprint not in TSS2_CAR:
     kiBP = [0., 5., 35.]
     kiV = [3.6, 2.4, 1.5]
@@ -454,7 +458,7 @@ class CarController(CarControllerBase):
         a_ego_future = a_ego_blended + j_ego * future_t
 
         if CC.longActive:
-          if should_bypass_toyota_long_pid(self.CP):
+          if should_bypass_toyota_long_pid(self.CP, starpilot_toggles):
             # Pedal/SDSU Toyotas have shown better behavior when we trust the planner
             # target directly instead of letting the Toyota longitudinal PID swing it
             # around. Keep the shared rate limits above, but bypass the extra
@@ -477,7 +481,7 @@ class CarController(CarControllerBase):
               pcm_accel_cmd += pitch_compensation
 
             feedforward = pcm_accel_cmd
-            if self.CP.carFingerprint == CAR.TOYOTA_PRIUS:
+            if self.CP.carFingerprint in LEGACY_PRIUS_CAR:
               feedforward = get_prius_feedforward(feedforward, CS.out.vEgo)
             elif is_camry_hybrid(self.CP) and feedforward > 0.0:
               # Preserve the established Camry Hybrid acceleration response while
@@ -507,7 +511,7 @@ class CarController(CarControllerBase):
         else:
           pcm_accel_cmd = limit_no_lead_cruise_sign_flip(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo,
                                                          CS.out.cruiseState.speed, bool(hud_control.leadVisible))
-          if self.CP.carFingerprint == CAR.TOYOTA_PRIUS:
+          if self.CP.carFingerprint in LEGACY_PRIUS_CAR:
             pcm_accel_cmd = limit_prius_stopping_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo, lead)
 
         pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))

opendbc_repo/opendbc/car/toyota/carstate.py

@@ -8,7 +8,7 @@ from opendbc.car.common.filter_simple import FirstOrderFilter
 from opendbc.car.interfaces import CarStateBase
 from opendbc.car.toyota.values import ToyotaFlags, ToyotaStarPilotFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
                                                   TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, \
-                                                  SECOC_CAR
+                                                  SECOC_CAR, LEGACY_PRIUS_CAR
 
 ButtonType = structs.CarState.ButtonEvent.Type
 SteerControlType = structs.CarParams.SteerControlType
@@ -23,7 +23,7 @@ TEMP_STEER_FAULTS = (0, 9, 11, 21, 25)
 # - lka/lta msg drop out: 3 (recoverable)
 # - prolonged high driver torque: 17 (permanent)
 PERM_STEER_FAULTS = (3, 17)
-LKAS_BUTTON_CAR = TSS2_CAR | {CAR.TOYOTA_PRIUS}
+LKAS_BUTTON_CAR = TSS2_CAR | LEGACY_PRIUS_CAR
 DISTANCE_BUTTON_CAR = {CAR.TOYOTA_SIENNA_4TH_GEN}
 
 

opendbc_repo/opendbc/car/toyota/fingerprints.py

@@ -6,6 +6,7 @@ Ecu = CarParams.Ecu
 
 FINGERPRINTS = {
   CAR.TOYOTA_MATRIX_RETROFIT: [{}],
+  CAR.TOYOTA_PRIUS_RETROFIT: [{}],
 }
 
 FW_VERSIONS = {

opendbc_repo/opendbc/car/toyota/interface.py

@@ -4,7 +4,7 @@ from opendbc.car.toyota.carcontroller import CarController
 from opendbc.car.toyota.radar_interface import RadarInterface
 from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, SECOC_CAR, NO_DSU_CAR, \
                                                   MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \
-                                                  ToyotaSafetyFlags
+                                                  ToyotaSafetyFlags, LEGACY_PRIUS_CAR
 from opendbc.car.disable_ecu import disable_ecu
 from opendbc.car.interfaces import CarInterfaceBase
 from opendbc.safety import ALTERNATIVE_EXPERIENCE
@@ -67,7 +67,7 @@ class CarInterface(CarInterfaceBase):
     # These messages are normally absent there on pre-TSS2 platforms.
     camera_fingerprint = fingerprint.get(2, {})
     has_dsu_bypass = 0x343 in camera_fingerprint or 0x4CB in camera_fingerprint
-    late_prius_camera = candidate == CAR.TOYOTA_PRIUS and any(
+    late_prius_camera = candidate in LEGACY_PRIUS_CAR and any(
       fw.ecu == Ecu.fwdCamera and bytes(fw.fwVersion).startswith(b'8646F4705') for fw in car_fw
     )
     if candidate in (CAR.LEXUS_IS, CAR.TOYOTA_CAMRY) or late_prius_camera:
@@ -83,7 +83,7 @@ class CarInterface(CarInterfaceBase):
     if Ecu.hybrid in found_ecus:
       ret.flags |= ToyotaFlags.HYBRID.value
 
-    if candidate == CAR.TOYOTA_PRIUS:
+    if candidate in LEGACY_PRIUS_CAR:
       stop_and_go = True
       ret.flags |= ToyotaFlags.HYBRID.value
       # Only give steer angle deadzone to for bad angle sensor prius
@@ -175,7 +175,7 @@ class CarInterface(CarInterfaceBase):
     # to a negative value, so it won't matter.
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptorDEPRECATED) else MIN_ACC_SPEED
 
-    prius_long_defaults = candidate == CAR.TOYOTA_PRIUS and ret.openpilotLongitudinalControl
+    prius_long_defaults = candidate in LEGACY_PRIUS_CAR and ret.openpilotLongitudinalControl
     camry_hybrid_long_defaults = (candidate == CAR.TOYOTA_CAMRY and ret.openpilotLongitudinalControl and
                                   bool(ret.flags & ToyotaFlags.HYBRID.value))
 

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

@@ -75,6 +75,22 @@ class TestToyotaInterfaces:
     assert default_params.lateralTuning.torque.steeringAngleDeadzoneDeg == pytest.approx(0.3)
     assert forced_params.lateralTuning.torque.steeringAngleDeadzoneDeg == pytest.approx(0.3)
 
+  def test_prius_tss2_eps_retrofit_uses_legacy_body_and_eps_scale(self):
+    params = CarInterface.get_params(
+      CAR.TOYOTA_PRIUS_RETROFIT,
+      {bus: {} for bus in range(8)},
+      [],
+      False,
+      False,
+      False,
+      SimpleNamespace(force_torque_controller=False, nnff=False, nnff_lite=False),
+    )
+
+    assert params.lateralTuning.which() == "torque"
+    assert params.safetyConfigs[0].safetyParam & 0xFF == 73
+    assert params.flags & ToyotaFlags.TSS2.value == 0
+    assert params.steerRatio == pytest.approx(15.74)
+
   def test_sienna_4th_gen_uses_torque_controller(self):
     params = CarInterface.get_params(
       CAR.TOYOTA_SIENNA_4TH_GEN,
@@ -469,6 +485,15 @@ class TestToyotaInterfaces:
 
     assert not should_bypass_toyota_long_pid(car_params)
 
+  def test_highlander_sdsu_bypasses_toyota_longitudinal_pid(self):
+    car_params = SimpleNamespace(
+      carFingerprint=CAR.TOYOTA_HIGHLANDER,
+      enableGasInterceptorDEPRECATED=False,
+    )
+
+    assert should_bypass_toyota_long_pid(car_params, SimpleNamespace(has_sdsu=True))
+    assert not should_bypass_toyota_long_pid(car_params, SimpleNamespace(has_sdsu=False))
+
   def test_camry_continental_radar_converts_absolute_target_speed(self):
     radar_interface = RadarInterface.__new__(RadarInterface)
     radar_interface.CP = SimpleNamespace(wheelSpeedFactor=1.0)

opendbc_repo/opendbc/car/toyota/values.py

@@ -250,6 +250,11 @@ class CAR(Platforms):
     CarSpecs(mass=3045. * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.74, tireStiffnessFactor=0.6371),
     dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
   )
+  TOYOTA_PRIUS_RETROFIT = PlatformConfig(
+    [ToyotaCommunityCarDocs("Toyota Prius 2016-20 with TSS2 EPS retrofit", package="Custom retrofit")],
+    TOYOTA_PRIUS.specs,
+    dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
+  )
   TOYOTA_PRIUS_V = PlatformConfig(
     [ToyotaCarDocs("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED)],
     CarSpecs(mass=3340. * CV.LB_TO_KG, wheelbase=2.78, steerRatio=17.4, tireStiffnessFactor=0.5533),
@@ -599,9 +604,11 @@ STEER_THRESHOLD = 100
 
 # These cars have non-standard EPS torque scale factors. All others are 73
 EPS_SCALE = defaultdict(lambda: 73,
-                        {CAR.TOYOTA_PRIUS: 66, CAR.TOYOTA_COROLLA: 88, CAR.TOYOTA_MATRIX_RETROFIT: 88,
+                        {CAR.TOYOTA_PRIUS: 66, CAR.TOYOTA_PRIUS_RETROFIT: 73, CAR.TOYOTA_COROLLA: 88, CAR.TOYOTA_MATRIX_RETROFIT: 88,
                          CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.TOYOTA_PRIUS_V: 100})
 
+LEGACY_PRIUS_CAR = frozenset((CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT))
+
 # Toyota/Lexus Safety Sense 2.0 and 2.5
 TSS2_CAR = CAR.with_flags(ToyotaFlags.TSS2)
 

opendbc_repo/opendbc/safety/modes/hyundai.h

@@ -90,7 +90,6 @@ static const CanMsg HYUNDAI_LONG_REFRESH_TX_MSGS[] = {
 
 static bool hyundai_legacy = false;
 static bool hyundai_can_canfd_blended_hda2 = false;
-static bool hyundai_acc_main_on_rx_prev = false;
 
 #define HYUNDAI_CAN_CANFD_BLENDED_HDA2_COMMON_RX_CHECKS()                                                                                                      \
   {.msg = {{0x260, 1, 8, 100U, .max_counter = 3U, .ignore_quality_flag = true},                                                                                \
@@ -194,12 +193,7 @@ static void hyundai_rx_hook(const CANPacket_t *msg) {
   if (msg->addr == 0x420U) {
     if (msg->bus == scc_bus) {
       if (!hyundai_longitudinal) {
-        const bool acc_main_on_rx = GET_BIT(msg, hyundai_can_canfd_blended ? 27U : 0U);
-        if (hyundai_aol_main_lkas_sync && (acc_main_on_rx != hyundai_acc_main_on_rx_prev)) {
-          lkas_on = false;
-        }
-        acc_main_on = acc_main_on_rx;
-        hyundai_acc_main_on_rx_prev = acc_main_on_rx;
+        acc_main_on = GET_BIT(msg, hyundai_can_canfd_blended ? 27U : 0U);
       }
     }
   }
@@ -448,8 +442,6 @@ static safety_config hyundai_init(uint16_t param) {
   hyundai_common_init(param);
   hyundai_legacy = false;
   hyundai_can_canfd_blended_hda2 = hyundai_can_canfd_blended && hyundai_canfd_lka_steering;
-  hyundai_aol_main_lkas_sync = GET_FLAG(param, 32U);
-  hyundai_acc_main_on_rx_prev = false;
 
   if (hyundai_can_canfd_blended) {
     gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut);

opendbc_repo/opendbc/safety/modes/hyundai_common.h

@@ -60,9 +60,6 @@ bool hyundai_cancel_button_enable = false;
 extern bool hyundai_can_refresh_msgs;
 bool hyundai_can_refresh_msgs = false;
 
-extern bool hyundai_aol_main_lkas_sync;
-bool hyundai_aol_main_lkas_sync = false;
-
 static uint8_t hyundai_last_button_interaction;  // button messages since the user pressed an enable button
 static bool acc_main_on_prev;
 static bool acc_main_on_tx;
@@ -98,7 +95,6 @@ void hyundai_common_init(uint16_t param) {
   hyundai_non_scc = GET_FLAG(param, HYUNDAI_PARAM_NON_SCC);
   hyundai_cancel_button_enable = GET_FLAG(param, HYUNDAI_PARAM_CANCEL_BTN_ENABLE);
   hyundai_can_refresh_msgs = GET_FLAG(param, HYUNDAI_PARAM_CAN_REFRESH_MSGS);
-  hyundai_aol_main_lkas_sync = false;
 
   hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
   acc_main_on_prev = false;
@@ -164,9 +160,7 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
   }
 
   if (main_button && !main_button_prev) {
-    if (!hyundai_aol_main_lkas_sync) {
-      acc_main_on = !acc_main_on;
-    }
+    acc_main_on = !acc_main_on;
   }
   main_button_prev = main_button;
 }

opendbc_repo/opendbc/safety/tests/test_hyundai.py

@@ -4,7 +4,6 @@ import unittest
 
 from opendbc.car.hyundai.values import HyundaiSafetyFlags, HyundaiStarPilotSafetyFlags
 from opendbc.car.structs import CarParams
-from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from opendbc.safety.tests.libsafety import libsafety_py
 import opendbc.safety.tests.common as common
 from opendbc.safety.tests.common import CANPackerSafety
@@ -622,65 +621,5 @@ class TestHyundaiAolLkasOnEngageStockSafety(HyundaiAolLkasOnEngageStockBase, Tes
     self.safety.init_tests()
 
 
-class TestHyundaiAolMainLkasSyncSafety(TestHyundaiSafety):
-  def setUp(self):
-    self.packer = CANPackerSafety("hyundai_kia_generic")
-    self.safety = libsafety_py.libsafety
-    self.safety.set_safety_hooks(
-      CarParams.SafetyModel.hyundai,
-      HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON | HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC,
-    )
-    self.safety.init_tests()
-
-  @staticmethod
-  def _lkas_button_msg(pressed):
-    dat = bytearray(8)
-    dat[0] = int(pressed) << 4
-    return libsafety_py.make_CANPacket(0x391, 0, bytes(dat))
-
-  def test_confirmed_main_state_rephases_lkas_button(self):
-    self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
-    self.safety.set_controls_allowed(False)
-
-    self._rx(self._lkas_button_msg(True))
-    self._rx(self._lkas_button_msg(False))
-    self.assertTrue(self.safety.get_lkas_on())
-    self.assertTrue(self.safety.get_aol_allowed())
-    self._set_prev_torque(0)
-    self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
-
-    self._rx(self._button_msg(Buttons.NONE, main_button=True))
-    self._rx(self._button_msg(Buttons.NONE, main_button=False))
-    self.assertFalse(self.safety.get_acc_main_on())
-    self.assertTrue(self.safety.get_lkas_on())
-    self.assertTrue(self.safety.get_aol_allowed())
-
-    self._rx(self._acc_state_msg(True))
-    self.assertFalse(self.safety.get_lkas_on())
-    self.assertTrue(self.safety.get_aol_allowed())
-    self._set_prev_torque(0)
-    self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
-
-    self._rx(self._button_msg(Buttons.NONE, main_button=True))
-    self._rx(self._button_msg(Buttons.NONE, main_button=False))
-    self.assertTrue(self.safety.get_acc_main_on())
-    self.assertTrue(self.safety.get_aol_allowed())
-
-    self._rx(self._acc_state_msg(False))
-    self.assertFalse(self.safety.get_controls_allowed())
-    self.assertFalse(self.safety.get_acc_main_on())
-    self.assertFalse(self.safety.get_lkas_on())
-    self.assertFalse(self.safety.get_aol_allowed())
-    self._set_prev_torque(0)
-    self.assertFalse(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
-
-    self._rx(self._lkas_button_msg(True))
-    self._rx(self._lkas_button_msg(False))
-    self.assertTrue(self.safety.get_lkas_on())
-    self.assertTrue(self.safety.get_aol_allowed())
-    self._set_prev_torque(0)
-    self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
-
-
 if __name__ == "__main__":
   unittest.main()

panda/board/obj/gitversion.h

@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-4078e6fc-DEBUG";
+const uint8_t gitversion[19] = "DEV-459d7c40-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-4078e6fc-DEBUG
+DEV-459d7c40-DEBUG

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -169,6 +169,7 @@ KONA_NON_SCC_CARS = (
 )
 PRIUS_CARS = (
   TOYOTA_CAR.TOYOTA_PRIUS,
+  TOYOTA_CAR.TOYOTA_PRIUS_RETROFIT,
 )
 
 CAMRY_CARS = (

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -627,11 +627,10 @@ class LongitudinalMpc:
     return lead_xv
 
   def process_lead(self, lead, tracking_lead=True, t_follow=None, *, lead_index=0,
-                   smooth_duplicate_vision=False, model_lead=None,
-                   use_model_lead_trajectory=False):
+                   smooth_duplicate_vision=False, model_lead=None):
     v_ego = self.x0[1]
     lead_active = lead is not None and lead.status and tracking_lead
-    if lead_active and use_model_lead_trajectory:
+    if lead_active:
       model_lead_xv = build_model_lead_trajectory(model_lead, lead, v_ego)
       if model_lead_xv is not None:
         return model_lead_xv
@@ -924,23 +923,18 @@ 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,
-             use_model_lead_trajectory=False):
+             stop_x=None, silverado_early_follow=False, modelV2=None):
     v_ego = self.x0[1]
     lead_one = radarstate.leadOne
     lead_two = radarstate.leadTwo
     self.status = tracking_lead and (lead_one.status or lead_two.status)
-    model_leads = ()
-    if use_model_lead_trajectory and modelV2 is not None:
-      model_leads = getattr(modelV2, "leadsV3", ())
+    model_leads = getattr(modelV2, "leadsV3", ()) if modelV2 is not None else ()
     lead_xv_0 = self.process_lead(lead_one, tracking_lead, t_follow=t_follow, lead_index=0,
                                   smooth_duplicate_vision=smooth_duplicate_vision,
-                                  model_lead=model_leads[0] if len(model_leads) > 0 else None,
-                                  use_model_lead_trajectory=use_model_lead_trajectory)
+                                  model_lead=model_leads[0] if len(model_leads) > 0 else None)
     lead_xv_1 = self.process_lead(lead_two, tracking_lead, t_follow=t_follow, lead_index=1,
                                   smooth_duplicate_vision=smooth_duplicate_vision,
-                                  model_lead=model_leads[1] if len(model_leads) > 1 else None,
-                                  use_model_lead_trajectory=use_model_lead_trajectory)
+                                  model_lead=model_leads[1] if len(model_leads) > 1 else None)
     self.lead_xv_0 = lead_xv_0
     self.lead_xv_1 = lead_xv_1
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -2208,8 +2208,7 @@ 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'],
-                    use_model_lead_trajectory=bool(getattr(starpilot_toggles, "test_model_lead_trajectory", False)))
+                    modelV2=sm['modelV2'])
 
     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/tests/test_longitudinal_planner.py

@@ -329,20 +329,16 @@ def make_model_lead(*, prob: float = 0.99, x=None, v=None):
   return model, lead
 
 
-def test_model_lead_trajectory_is_opt_in_and_disabled_path_is_unchanged():
+def test_model_lead_trajectory_is_default_for_stable_lead():
   lead = make_lead(status=True, d_rel=42.0, v_lead=18.0, model_prob=0.99)
   _, model_lead = make_model_lead()
 
-  legacy_mpc = LongitudinalMpc()
-  disabled_mpc = LongitudinalMpc()
-  legacy_mpc.set_cur_state(20.0, 0.0)
-  disabled_mpc.set_cur_state(20.0, 0.0)
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(20.0, 0.0)
 
-  legacy = legacy_mpc.process_lead(lead)
-  disabled = disabled_mpc.process_lead(
-    lead, model_lead=model_lead, use_model_lead_trajectory=False,
-  )
-  np.testing.assert_allclose(disabled, legacy)
+  actual = mpc.process_lead(lead, model_lead=model_lead)
+  expected = build_model_lead_trajectory(model_lead, lead, 20.0)
+  np.testing.assert_allclose(actual, expected)
 
 
 def test_model_lead_trajectory_uses_raw_current_anchor_and_future_deltas():

selfdrive/controls/tests/test_starpilot_acceleration.py

@@ -221,14 +221,17 @@ def test_pulse_and_glide_coasts_at_set_speed_then_resumes_below_delta():
 
   accel.update(set_speed, make_sm(set_speed_kph=100.0, pulse_and_glide=True), toggles)
   assert accel.pulse_glide_coasting is True
+  assert accel.pulse_glide_target == pytest.approx(set_speed - delta)
   assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO)
 
   accel.update((90.0 * CV.KPH_TO_MS) - 0.05, make_sm(set_speed_kph=100.0, pulse_and_glide=True), toggles)
   assert accel.pulse_glide_coasting is False
+  assert accel.pulse_glide_target is None
   assert accel.min_accel == pytest.approx(A_CRUISE_MIN)
 
   accel.update(99.8 * CV.KPH_TO_MS, make_sm(set_speed_kph=100.0, pulse_and_glide=True), toggles)
   assert accel.pulse_glide_coasting is True
+  assert accel.pulse_glide_target == pytest.approx(set_speed - delta)
   assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO)
 
 
@@ -239,4 +242,5 @@ def test_pulse_and_glide_is_inert_when_disabled():
   accel.update(100.0 * CV.KPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["STANDARD"], pulse_glide_speed_delta=10.0))
 
   assert accel.pulse_glide_coasting is False
+  assert accel.pulse_glide_target is None
   assert accel.min_accel == pytest.approx(A_CRUISE_MIN)

selfdrive/controls/tests/test_starpilot_planner.py

@@ -2,6 +2,7 @@ import math
 from pathlib import Path
 from types import SimpleNamespace
 
+from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
 from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner, get_force_stop_jerk_scale
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_lead_follow_jerk_scale
@@ -119,6 +120,25 @@ def test_standstill_without_turn_signal_keeps_lateral_allowed(monkeypatch):
     planner.shutdown()
 
 
+def test_pulse_glide_target_is_published_after_vcruise_update(monkeypatch):
+  planner = make_planner(monkeypatch)
+
+  try:
+    normal_target = 65.0 * CV.MPH_TO_MS
+    glide_target = 60.0 * CV.MPH_TO_MS
+    monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: normal_target)
+
+    def publish_glide_target(*args, **kwargs):
+      planner.starpilot_acceleration.pulse_glide_target = glide_target
+
+    monkeypatch.setattr(planner.starpilot_acceleration, "update", publish_glide_target)
+    planner.update(0.0, False, make_sm(planner, frame=1, v_ego=normal_target, left_blinker=False), make_toggles())
+
+    assert planner.v_cruise == glide_target
+  finally:
+    planner.shutdown()
+
+
 def test_lateral_resume_delay_holds_resume_after_low_speed_turn(monkeypatch):
   planner = make_planner(monkeypatch)
 

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_555027713999943867) {
-   out_555027713999943867[0] = delta_x[0] + nom_x[0];
-   out_555027713999943867[1] = delta_x[1] + nom_x[1];
-   out_555027713999943867[2] = delta_x[2] + nom_x[2];
-   out_555027713999943867[3] = delta_x[3] + nom_x[3];
-   out_555027713999943867[4] = delta_x[4] + nom_x[4];
-   out_555027713999943867[5] = delta_x[5] + nom_x[5];
-   out_555027713999943867[6] = delta_x[6] + nom_x[6];
-   out_555027713999943867[7] = delta_x[7] + nom_x[7];
-   out_555027713999943867[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_3745689373699638561) {
+   out_3745689373699638561[0] = delta_x[0] + nom_x[0];
+   out_3745689373699638561[1] = delta_x[1] + nom_x[1];
+   out_3745689373699638561[2] = delta_x[2] + nom_x[2];
+   out_3745689373699638561[3] = delta_x[3] + nom_x[3];
+   out_3745689373699638561[4] = delta_x[4] + nom_x[4];
+   out_3745689373699638561[5] = delta_x[5] + nom_x[5];
+   out_3745689373699638561[6] = delta_x[6] + nom_x[6];
+   out_3745689373699638561[7] = delta_x[7] + nom_x[7];
+   out_3745689373699638561[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_7816877518147233589) {
-   out_7816877518147233589[0] = -nom_x[0] + true_x[0];
-   out_7816877518147233589[1] = -nom_x[1] + true_x[1];
-   out_7816877518147233589[2] = -nom_x[2] + true_x[2];
-   out_7816877518147233589[3] = -nom_x[3] + true_x[3];
-   out_7816877518147233589[4] = -nom_x[4] + true_x[4];
-   out_7816877518147233589[5] = -nom_x[5] + true_x[5];
-   out_7816877518147233589[6] = -nom_x[6] + true_x[6];
-   out_7816877518147233589[7] = -nom_x[7] + true_x[7];
-   out_7816877518147233589[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_2400897459040094715) {
+   out_2400897459040094715[0] = -nom_x[0] + true_x[0];
+   out_2400897459040094715[1] = -nom_x[1] + true_x[1];
+   out_2400897459040094715[2] = -nom_x[2] + true_x[2];
+   out_2400897459040094715[3] = -nom_x[3] + true_x[3];
+   out_2400897459040094715[4] = -nom_x[4] + true_x[4];
+   out_2400897459040094715[5] = -nom_x[5] + true_x[5];
+   out_2400897459040094715[6] = -nom_x[6] + true_x[6];
+   out_2400897459040094715[7] = -nom_x[7] + true_x[7];
+   out_2400897459040094715[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_4533074481319696241) {
-   out_4533074481319696241[0] = 1.0;
-   out_4533074481319696241[1] = 0.0;
-   out_4533074481319696241[2] = 0.0;
-   out_4533074481319696241[3] = 0.0;
-   out_4533074481319696241[4] = 0.0;
-   out_4533074481319696241[5] = 0.0;
-   out_4533074481319696241[6] = 0.0;
-   out_4533074481319696241[7] = 0.0;
-   out_4533074481319696241[8] = 0.0;
-   out_4533074481319696241[9] = 0.0;
-   out_4533074481319696241[10] = 1.0;
-   out_4533074481319696241[11] = 0.0;
-   out_4533074481319696241[12] = 0.0;
-   out_4533074481319696241[13] = 0.0;
-   out_4533074481319696241[14] = 0.0;
-   out_4533074481319696241[15] = 0.0;
-   out_4533074481319696241[16] = 0.0;
-   out_4533074481319696241[17] = 0.0;
-   out_4533074481319696241[18] = 0.0;
-   out_4533074481319696241[19] = 0.0;
-   out_4533074481319696241[20] = 1.0;
-   out_4533074481319696241[21] = 0.0;
-   out_4533074481319696241[22] = 0.0;
-   out_4533074481319696241[23] = 0.0;
-   out_4533074481319696241[24] = 0.0;
-   out_4533074481319696241[25] = 0.0;
-   out_4533074481319696241[26] = 0.0;
-   out_4533074481319696241[27] = 0.0;
-   out_4533074481319696241[28] = 0.0;
-   out_4533074481319696241[29] = 0.0;
-   out_4533074481319696241[30] = 1.0;
-   out_4533074481319696241[31] = 0.0;
-   out_4533074481319696241[32] = 0.0;
-   out_4533074481319696241[33] = 0.0;
-   out_4533074481319696241[34] = 0.0;
-   out_4533074481319696241[35] = 0.0;
-   out_4533074481319696241[36] = 0.0;
-   out_4533074481319696241[37] = 0.0;
-   out_4533074481319696241[38] = 0.0;
-   out_4533074481319696241[39] = 0.0;
-   out_4533074481319696241[40] = 1.0;
-   out_4533074481319696241[41] = 0.0;
-   out_4533074481319696241[42] = 0.0;
-   out_4533074481319696241[43] = 0.0;
-   out_4533074481319696241[44] = 0.0;
-   out_4533074481319696241[45] = 0.0;
-   out_4533074481319696241[46] = 0.0;
-   out_4533074481319696241[47] = 0.0;
-   out_4533074481319696241[48] = 0.0;
-   out_4533074481319696241[49] = 0.0;
-   out_4533074481319696241[50] = 1.0;
-   out_4533074481319696241[51] = 0.0;
-   out_4533074481319696241[52] = 0.0;
-   out_4533074481319696241[53] = 0.0;
-   out_4533074481319696241[54] = 0.0;
-   out_4533074481319696241[55] = 0.0;
-   out_4533074481319696241[56] = 0.0;
-   out_4533074481319696241[57] = 0.0;
-   out_4533074481319696241[58] = 0.0;
-   out_4533074481319696241[59] = 0.0;
-   out_4533074481319696241[60] = 1.0;
-   out_4533074481319696241[61] = 0.0;
-   out_4533074481319696241[62] = 0.0;
-   out_4533074481319696241[63] = 0.0;
-   out_4533074481319696241[64] = 0.0;
-   out_4533074481319696241[65] = 0.0;
-   out_4533074481319696241[66] = 0.0;
-   out_4533074481319696241[67] = 0.0;
-   out_4533074481319696241[68] = 0.0;
-   out_4533074481319696241[69] = 0.0;
-   out_4533074481319696241[70] = 1.0;
-   out_4533074481319696241[71] = 0.0;
-   out_4533074481319696241[72] = 0.0;
-   out_4533074481319696241[73] = 0.0;
-   out_4533074481319696241[74] = 0.0;
-   out_4533074481319696241[75] = 0.0;
-   out_4533074481319696241[76] = 0.0;
-   out_4533074481319696241[77] = 0.0;
-   out_4533074481319696241[78] = 0.0;
-   out_4533074481319696241[79] = 0.0;
-   out_4533074481319696241[80] = 1.0;
+void H_mod_fun(double *state, double *out_7767655668809768572) {
+   out_7767655668809768572[0] = 1.0;
+   out_7767655668809768572[1] = 0.0;
+   out_7767655668809768572[2] = 0.0;
+   out_7767655668809768572[3] = 0.0;
+   out_7767655668809768572[4] = 0.0;
+   out_7767655668809768572[5] = 0.0;
+   out_7767655668809768572[6] = 0.0;
+   out_7767655668809768572[7] = 0.0;
+   out_7767655668809768572[8] = 0.0;
+   out_7767655668809768572[9] = 0.0;
+   out_7767655668809768572[10] = 1.0;
+   out_7767655668809768572[11] = 0.0;
+   out_7767655668809768572[12] = 0.0;
+   out_7767655668809768572[13] = 0.0;
+   out_7767655668809768572[14] = 0.0;
+   out_7767655668809768572[15] = 0.0;
+   out_7767655668809768572[16] = 0.0;
+   out_7767655668809768572[17] = 0.0;
+   out_7767655668809768572[18] = 0.0;
+   out_7767655668809768572[19] = 0.0;
+   out_7767655668809768572[20] = 1.0;
+   out_7767655668809768572[21] = 0.0;
+   out_7767655668809768572[22] = 0.0;
+   out_7767655668809768572[23] = 0.0;
+   out_7767655668809768572[24] = 0.0;
+   out_7767655668809768572[25] = 0.0;
+   out_7767655668809768572[26] = 0.0;
+   out_7767655668809768572[27] = 0.0;
+   out_7767655668809768572[28] = 0.0;
+   out_7767655668809768572[29] = 0.0;
+   out_7767655668809768572[30] = 1.0;
+   out_7767655668809768572[31] = 0.0;
+   out_7767655668809768572[32] = 0.0;
+   out_7767655668809768572[33] = 0.0;
+   out_7767655668809768572[34] = 0.0;
+   out_7767655668809768572[35] = 0.0;
+   out_7767655668809768572[36] = 0.0;
+   out_7767655668809768572[37] = 0.0;
+   out_7767655668809768572[38] = 0.0;
+   out_7767655668809768572[39] = 0.0;
+   out_7767655668809768572[40] = 1.0;
+   out_7767655668809768572[41] = 0.0;
+   out_7767655668809768572[42] = 0.0;
+   out_7767655668809768572[43] = 0.0;
+   out_7767655668809768572[44] = 0.0;
+   out_7767655668809768572[45] = 0.0;
+   out_7767655668809768572[46] = 0.0;
+   out_7767655668809768572[47] = 0.0;
+   out_7767655668809768572[48] = 0.0;
+   out_7767655668809768572[49] = 0.0;
+   out_7767655668809768572[50] = 1.0;
+   out_7767655668809768572[51] = 0.0;
+   out_7767655668809768572[52] = 0.0;
+   out_7767655668809768572[53] = 0.0;
+   out_7767655668809768572[54] = 0.0;
+   out_7767655668809768572[55] = 0.0;
+   out_7767655668809768572[56] = 0.0;
+   out_7767655668809768572[57] = 0.0;
+   out_7767655668809768572[58] = 0.0;
+   out_7767655668809768572[59] = 0.0;
+   out_7767655668809768572[60] = 1.0;
+   out_7767655668809768572[61] = 0.0;
+   out_7767655668809768572[62] = 0.0;
+   out_7767655668809768572[63] = 0.0;
+   out_7767655668809768572[64] = 0.0;
+   out_7767655668809768572[65] = 0.0;
+   out_7767655668809768572[66] = 0.0;
+   out_7767655668809768572[67] = 0.0;
+   out_7767655668809768572[68] = 0.0;
+   out_7767655668809768572[69] = 0.0;
+   out_7767655668809768572[70] = 1.0;
+   out_7767655668809768572[71] = 0.0;
+   out_7767655668809768572[72] = 0.0;
+   out_7767655668809768572[73] = 0.0;
+   out_7767655668809768572[74] = 0.0;
+   out_7767655668809768572[75] = 0.0;
+   out_7767655668809768572[76] = 0.0;
+   out_7767655668809768572[77] = 0.0;
+   out_7767655668809768572[78] = 0.0;
+   out_7767655668809768572[79] = 0.0;
+   out_7767655668809768572[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_5039337670867030572) {
-   out_5039337670867030572[0] = state[0];
-   out_5039337670867030572[1] = state[1];
-   out_5039337670867030572[2] = state[2];
-   out_5039337670867030572[3] = state[3];
-   out_5039337670867030572[4] = state[4];
-   out_5039337670867030572[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_5039337670867030572[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_5039337670867030572[7] = state[7];
-   out_5039337670867030572[8] = state[8];
+void f_fun(double *state, double dt, double *out_3612821593330881774) {
+   out_3612821593330881774[0] = state[0];
+   out_3612821593330881774[1] = state[1];
+   out_3612821593330881774[2] = state[2];
+   out_3612821593330881774[3] = state[3];
+   out_3612821593330881774[4] = state[4];
+   out_3612821593330881774[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_3612821593330881774[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_3612821593330881774[7] = state[7];
+   out_3612821593330881774[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_5181832378332727216) {
-   out_5181832378332727216[0] = 1;
-   out_5181832378332727216[1] = 0;
-   out_5181832378332727216[2] = 0;
-   out_5181832378332727216[3] = 0;
-   out_5181832378332727216[4] = 0;
-   out_5181832378332727216[5] = 0;
-   out_5181832378332727216[6] = 0;
-   out_5181832378332727216[7] = 0;
-   out_5181832378332727216[8] = 0;
-   out_5181832378332727216[9] = 0;
-   out_5181832378332727216[10] = 1;
-   out_5181832378332727216[11] = 0;
-   out_5181832378332727216[12] = 0;
-   out_5181832378332727216[13] = 0;
-   out_5181832378332727216[14] = 0;
-   out_5181832378332727216[15] = 0;
-   out_5181832378332727216[16] = 0;
-   out_5181832378332727216[17] = 0;
-   out_5181832378332727216[18] = 0;
-   out_5181832378332727216[19] = 0;
-   out_5181832378332727216[20] = 1;
-   out_5181832378332727216[21] = 0;
-   out_5181832378332727216[22] = 0;
-   out_5181832378332727216[23] = 0;
-   out_5181832378332727216[24] = 0;
-   out_5181832378332727216[25] = 0;
-   out_5181832378332727216[26] = 0;
-   out_5181832378332727216[27] = 0;
-   out_5181832378332727216[28] = 0;
-   out_5181832378332727216[29] = 0;
-   out_5181832378332727216[30] = 1;
-   out_5181832378332727216[31] = 0;
-   out_5181832378332727216[32] = 0;
-   out_5181832378332727216[33] = 0;
-   out_5181832378332727216[34] = 0;
-   out_5181832378332727216[35] = 0;
-   out_5181832378332727216[36] = 0;
-   out_5181832378332727216[37] = 0;
-   out_5181832378332727216[38] = 0;
-   out_5181832378332727216[39] = 0;
-   out_5181832378332727216[40] = 1;
-   out_5181832378332727216[41] = 0;
-   out_5181832378332727216[42] = 0;
-   out_5181832378332727216[43] = 0;
-   out_5181832378332727216[44] = 0;
-   out_5181832378332727216[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_5181832378332727216[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_5181832378332727216[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_5181832378332727216[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_5181832378332727216[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_5181832378332727216[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_5181832378332727216[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_5181832378332727216[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_5181832378332727216[53] = -9.8100000000000005*dt;
-   out_5181832378332727216[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_5181832378332727216[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_5181832378332727216[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_5181832378332727216[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_5181832378332727216[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_5181832378332727216[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_5181832378332727216[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_5181832378332727216[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_5181832378332727216[62] = 0;
-   out_5181832378332727216[63] = 0;
-   out_5181832378332727216[64] = 0;
-   out_5181832378332727216[65] = 0;
-   out_5181832378332727216[66] = 0;
-   out_5181832378332727216[67] = 0;
-   out_5181832378332727216[68] = 0;
-   out_5181832378332727216[69] = 0;
-   out_5181832378332727216[70] = 1;
-   out_5181832378332727216[71] = 0;
-   out_5181832378332727216[72] = 0;
-   out_5181832378332727216[73] = 0;
-   out_5181832378332727216[74] = 0;
-   out_5181832378332727216[75] = 0;
-   out_5181832378332727216[76] = 0;
-   out_5181832378332727216[77] = 0;
-   out_5181832378332727216[78] = 0;
-   out_5181832378332727216[79] = 0;
-   out_5181832378332727216[80] = 1;
+void F_fun(double *state, double dt, double *out_2179834667538047746) {
+   out_2179834667538047746[0] = 1;
+   out_2179834667538047746[1] = 0;
+   out_2179834667538047746[2] = 0;
+   out_2179834667538047746[3] = 0;
+   out_2179834667538047746[4] = 0;
+   out_2179834667538047746[5] = 0;
+   out_2179834667538047746[6] = 0;
+   out_2179834667538047746[7] = 0;
+   out_2179834667538047746[8] = 0;
+   out_2179834667538047746[9] = 0;
+   out_2179834667538047746[10] = 1;
+   out_2179834667538047746[11] = 0;
+   out_2179834667538047746[12] = 0;
+   out_2179834667538047746[13] = 0;
+   out_2179834667538047746[14] = 0;
+   out_2179834667538047746[15] = 0;
+   out_2179834667538047746[16] = 0;
+   out_2179834667538047746[17] = 0;
+   out_2179834667538047746[18] = 0;
+   out_2179834667538047746[19] = 0;
+   out_2179834667538047746[20] = 1;
+   out_2179834667538047746[21] = 0;
+   out_2179834667538047746[22] = 0;
+   out_2179834667538047746[23] = 0;
+   out_2179834667538047746[24] = 0;
+   out_2179834667538047746[25] = 0;
+   out_2179834667538047746[26] = 0;
+   out_2179834667538047746[27] = 0;
+   out_2179834667538047746[28] = 0;
+   out_2179834667538047746[29] = 0;
+   out_2179834667538047746[30] = 1;
+   out_2179834667538047746[31] = 0;
+   out_2179834667538047746[32] = 0;
+   out_2179834667538047746[33] = 0;
+   out_2179834667538047746[34] = 0;
+   out_2179834667538047746[35] = 0;
+   out_2179834667538047746[36] = 0;
+   out_2179834667538047746[37] = 0;
+   out_2179834667538047746[38] = 0;
+   out_2179834667538047746[39] = 0;
+   out_2179834667538047746[40] = 1;
+   out_2179834667538047746[41] = 0;
+   out_2179834667538047746[42] = 0;
+   out_2179834667538047746[43] = 0;
+   out_2179834667538047746[44] = 0;
+   out_2179834667538047746[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_2179834667538047746[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_2179834667538047746[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_2179834667538047746[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_2179834667538047746[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_2179834667538047746[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_2179834667538047746[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_2179834667538047746[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_2179834667538047746[53] = -9.8100000000000005*dt;
+   out_2179834667538047746[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_2179834667538047746[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_2179834667538047746[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_2179834667538047746[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_2179834667538047746[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_2179834667538047746[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_2179834667538047746[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_2179834667538047746[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_2179834667538047746[62] = 0;
+   out_2179834667538047746[63] = 0;
+   out_2179834667538047746[64] = 0;
+   out_2179834667538047746[65] = 0;
+   out_2179834667538047746[66] = 0;
+   out_2179834667538047746[67] = 0;
+   out_2179834667538047746[68] = 0;
+   out_2179834667538047746[69] = 0;
+   out_2179834667538047746[70] = 1;
+   out_2179834667538047746[71] = 0;
+   out_2179834667538047746[72] = 0;
+   out_2179834667538047746[73] = 0;
+   out_2179834667538047746[74] = 0;
+   out_2179834667538047746[75] = 0;
+   out_2179834667538047746[76] = 0;
+   out_2179834667538047746[77] = 0;
+   out_2179834667538047746[78] = 0;
+   out_2179834667538047746[79] = 0;
+   out_2179834667538047746[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_9064502358369772671) {
-   out_9064502358369772671[0] = state[6];
+void h_25(double *state, double *unused, double *out_6182136655387767236) {
+   out_6182136655387767236[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_9009977767205190535) {
-   out_9009977767205190535[0] = 0;
-   out_9009977767205190535[1] = 0;
-   out_9009977767205190535[2] = 0;
-   out_9009977767205190535[3] = 0;
-   out_9009977767205190535[4] = 0;
-   out_9009977767205190535[5] = 0;
-   out_9009977767205190535[6] = 1;
-   out_9009977767205190535[7] = 0;
-   out_9009977767205190535[8] = 0;
+void H_25(double *state, double *unused, double *out_2863963843625103732) {
+   out_2863963843625103732[0] = 0;
+   out_2863963843625103732[1] = 0;
+   out_2863963843625103732[2] = 0;
+   out_2863963843625103732[3] = 0;
+   out_2863963843625103732[4] = 0;
+   out_2863963843625103732[5] = 0;
+   out_2863963843625103732[6] = 1;
+   out_2863963843625103732[7] = 0;
+   out_2863963843625103732[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_4315668618686905201) {
-   out_4315668618686905201[0] = state[4];
-   out_4315668618686905201[1] = state[5];
+void h_24(double *state, double *unused, double *out_7699556082354746130) {
+   out_7699556082354746130[0] = state[4];
+   out_7699556082354746130[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_4579303311377586733) {
-   out_4579303311377586733[0] = 0;
-   out_4579303311377586733[1] = 0;
-   out_4579303311377586733[2] = 0;
-   out_4579303311377586733[3] = 0;
-   out_4579303311377586733[4] = 1;
-   out_4579303311377586733[5] = 0;
-   out_4579303311377586733[6] = 0;
-   out_4579303311377586733[7] = 0;
-   out_4579303311377586733[8] = 0;
-   out_4579303311377586733[9] = 0;
-   out_4579303311377586733[10] = 0;
-   out_4579303311377586733[11] = 0;
-   out_4579303311377586733[12] = 0;
-   out_4579303311377586733[13] = 0;
-   out_4579303311377586733[14] = 1;
-   out_4579303311377586733[15] = 0;
-   out_4579303311377586733[16] = 0;
-   out_4579303311377586733[17] = 0;
+void H_24(double *state, double *unused, double *out_8991184345786785929) {
+   out_8991184345786785929[0] = 0;
+   out_8991184345786785929[1] = 0;
+   out_8991184345786785929[2] = 0;
+   out_8991184345786785929[3] = 0;
+   out_8991184345786785929[4] = 1;
+   out_8991184345786785929[5] = 0;
+   out_8991184345786785929[6] = 0;
+   out_8991184345786785929[7] = 0;
+   out_8991184345786785929[8] = 0;
+   out_8991184345786785929[9] = 0;
+   out_8991184345786785929[10] = 0;
+   out_8991184345786785929[11] = 0;
+   out_8991184345786785929[12] = 0;
+   out_8991184345786785929[13] = 0;
+   out_8991184345786785929[14] = 1;
+   out_8991184345786785929[15] = 0;
+   out_8991184345786785929[16] = 0;
+   out_8991184345786785929[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_8507922675073509890) {
-   out_8507922675073509890[0] = state[4];
+void h_30(double *state, double *unused, double *out_3234410665032282275) {
+   out_3234410665032282275[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_2093287425713573780) {
-   out_2093287425713573780[0] = 0;
-   out_2093287425713573780[1] = 0;
-   out_2093287425713573780[2] = 0;
-   out_2093287425713573780[3] = 0;
-   out_2093287425713573780[4] = 1;
-   out_2093287425713573780[5] = 0;
-   out_2093287425713573780[6] = 0;
-   out_2093287425713573780[7] = 0;
-   out_2093287425713573780[8] = 0;
+void H_30(double *state, double *unused, double *out_345630885117855105) {
+   out_345630885117855105[0] = 0;
+   out_345630885117855105[1] = 0;
+   out_345630885117855105[2] = 0;
+   out_345630885117855105[3] = 0;
+   out_345630885117855105[4] = 1;
+   out_345630885117855105[5] = 0;
+   out_345630885117855105[6] = 0;
+   out_345630885117855105[7] = 0;
+   out_345630885117855105[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_4180836484642303770) {
-   out_4180836484642303770[0] = state[7];
+void h_26(double *state, double *unused, double *out_8236083932693127920) {
+   out_8236083932693127920[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_5695262987630304857) {
-   out_5695262987630304857[0] = 0;
-   out_5695262987630304857[1] = 0;
-   out_5695262987630304857[2] = 0;
-   out_5695262987630304857[3] = 0;
-   out_5695262987630304857[4] = 0;
-   out_5695262987630304857[5] = 0;
-   out_5695262987630304857[6] = 0;
-   out_5695262987630304857[7] = 1;
-   out_5695262987630304857[8] = 0;
+void H_26(double *state, double *unused, double *out_6605467162499159956) {
+   out_6605467162499159956[0] = 0;
+   out_6605467162499159956[1] = 0;
+   out_6605467162499159956[2] = 0;
+   out_6605467162499159956[3] = 0;
+   out_6605467162499159956[4] = 0;
+   out_6605467162499159956[5] = 0;
+   out_6605467162499159956[6] = 0;
+   out_6605467162499159956[7] = 1;
+   out_6605467162499159956[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_4062829090708927026) {
-   out_4062829090708927026[0] = state[3];
+void h_27(double *state, double *unused, double *out_5200052562052469124) {
+   out_5200052562052469124[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_4268050737513998691) {
-   out_4268050737513998691[0] = 0;
-   out_4268050737513998691[1] = 0;
-   out_4268050737513998691[2] = 0;
-   out_4268050737513998691[3] = 1;
-   out_4268050737513998691[4] = 0;
-   out_4268050737513998691[5] = 0;
-   out_4268050737513998691[6] = 0;
-   out_4268050737513998691[7] = 0;
-   out_4268050737513998691[8] = 0;
+void H_27(double *state, double *unused, double *out_1877963186066088112) {
+   out_1877963186066088112[0] = 0;
+   out_1877963186066088112[1] = 0;
+   out_1877963186066088112[2] = 0;
+   out_1877963186066088112[3] = 1;
+   out_1877963186066088112[4] = 0;
+   out_1877963186066088112[5] = 0;
+   out_1877963186066088112[6] = 0;
+   out_1877963186066088112[7] = 0;
+   out_1877963186066088112[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_7564412822171847455) {
-   out_7564412822171847455[0] = state[1];
+void h_29(double *state, double *unused, double *out_1767262602628563370) {
+   out_1767262602628563370[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_5981413464383549724) {
-   out_5981413464383549724[0] = 0;
-   out_5981413464383549724[1] = 1;
-   out_5981413464383549724[2] = 0;
-   out_5981413464383549724[3] = 0;
-   out_5981413464383549724[4] = 0;
-   out_5981413464383549724[5] = 0;
-   out_5981413464383549724[6] = 0;
-   out_5981413464383549724[7] = 0;
-   out_5981413464383549724[8] = 0;
+void H_29(double *state, double *unused, double *out_164600459196537079) {
+   out_164600459196537079[0] = 0;
+   out_164600459196537079[1] = 1;
+   out_164600459196537079[2] = 0;
+   out_164600459196537079[3] = 0;
+   out_164600459196537079[4] = 0;
+   out_164600459196537079[5] = 0;
+   out_164600459196537079[6] = 0;
+   out_164600459196537079[7] = 0;
+   out_164600459196537079[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_1579900780372029809) {
-   out_1579900780372029809[0] = state[0];
+void h_28(double *state, double *unused, double *out_8284311513975789260) {
+   out_8284311513975789260[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_7382931592256471318) {
-   out_7382931592256471318[0] = 1;
-   out_7382931592256471318[1] = 0;
-   out_7382931592256471318[2] = 0;
-   out_7382931592256471318[3] = 0;
-   out_7382931592256471318[4] = 0;
-   out_7382931592256471318[5] = 0;
-   out_7382931592256471318[6] = 0;
-   out_7382931592256471318[7] = 0;
-   out_7382931592256471318[8] = 0;
+void H_28(double *state, double *unused, double *out_4917798557872993495) {
+   out_4917798557872993495[0] = 1;
+   out_4917798557872993495[1] = 0;
+   out_4917798557872993495[2] = 0;
+   out_4917798557872993495[3] = 0;
+   out_4917798557872993495[4] = 0;
+   out_4917798557872993495[5] = 0;
+   out_4917798557872993495[6] = 0;
+   out_4917798557872993495[7] = 0;
+   out_4917798557872993495[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_6613551970606485049) {
-   out_6613551970606485049[0] = state[8];
+void h_31(double *state, double *unused, double *out_588698570962583700) {
+   out_588698570962583700[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_8979331805328230107) {
-   out_8979331805328230107[0] = 0;
-   out_8979331805328230107[1] = 0;
-   out_8979331805328230107[2] = 0;
-   out_8979331805328230107[3] = 0;
-   out_8979331805328230107[4] = 0;
-   out_8979331805328230107[5] = 0;
-   out_8979331805328230107[6] = 0;
-   out_8979331805328230107[7] = 0;
-   out_8979331805328230107[8] = 1;
+void H_31(double *state, double *unused, double *out_7231675264732511432) {
+   out_7231675264732511432[0] = 0;
+   out_7231675264732511432[1] = 0;
+   out_7231675264732511432[2] = 0;
+   out_7231675264732511432[3] = 0;
+   out_7231675264732511432[4] = 0;
+   out_7231675264732511432[5] = 0;
+   out_7231675264732511432[6] = 0;
+   out_7231675264732511432[7] = 0;
+   out_7231675264732511432[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_555027713999943867) {
-  err_fun(nom_x, delta_x, out_555027713999943867);
+void car_err_fun(double *nom_x, double *delta_x, double *out_3745689373699638561) {
+  err_fun(nom_x, delta_x, out_3745689373699638561);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_7816877518147233589) {
-  inv_err_fun(nom_x, true_x, out_7816877518147233589);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_2400897459040094715) {
+  inv_err_fun(nom_x, true_x, out_2400897459040094715);
 }
-void car_H_mod_fun(double *state, double *out_4533074481319696241) {
-  H_mod_fun(state, out_4533074481319696241);
+void car_H_mod_fun(double *state, double *out_7767655668809768572) {
+  H_mod_fun(state, out_7767655668809768572);
 }
-void car_f_fun(double *state, double dt, double *out_5039337670867030572) {
-  f_fun(state,  dt, out_5039337670867030572);
+void car_f_fun(double *state, double dt, double *out_3612821593330881774) {
+  f_fun(state,  dt, out_3612821593330881774);
 }
-void car_F_fun(double *state, double dt, double *out_5181832378332727216) {
-  F_fun(state,  dt, out_5181832378332727216);
+void car_F_fun(double *state, double dt, double *out_2179834667538047746) {
+  F_fun(state,  dt, out_2179834667538047746);
 }
-void car_h_25(double *state, double *unused, double *out_9064502358369772671) {
-  h_25(state, unused, out_9064502358369772671);
+void car_h_25(double *state, double *unused, double *out_6182136655387767236) {
+  h_25(state, unused, out_6182136655387767236);
 }
-void car_H_25(double *state, double *unused, double *out_9009977767205190535) {
-  H_25(state, unused, out_9009977767205190535);
+void car_H_25(double *state, double *unused, double *out_2863963843625103732) {
+  H_25(state, unused, out_2863963843625103732);
 }
-void car_h_24(double *state, double *unused, double *out_4315668618686905201) {
-  h_24(state, unused, out_4315668618686905201);
+void car_h_24(double *state, double *unused, double *out_7699556082354746130) {
+  h_24(state, unused, out_7699556082354746130);
 }
-void car_H_24(double *state, double *unused, double *out_4579303311377586733) {
-  H_24(state, unused, out_4579303311377586733);
+void car_H_24(double *state, double *unused, double *out_8991184345786785929) {
+  H_24(state, unused, out_8991184345786785929);
 }
-void car_h_30(double *state, double *unused, double *out_8507922675073509890) {
-  h_30(state, unused, out_8507922675073509890);
+void car_h_30(double *state, double *unused, double *out_3234410665032282275) {
+  h_30(state, unused, out_3234410665032282275);
 }
-void car_H_30(double *state, double *unused, double *out_2093287425713573780) {
-  H_30(state, unused, out_2093287425713573780);
+void car_H_30(double *state, double *unused, double *out_345630885117855105) {
+  H_30(state, unused, out_345630885117855105);
 }
-void car_h_26(double *state, double *unused, double *out_4180836484642303770) {
-  h_26(state, unused, out_4180836484642303770);
+void car_h_26(double *state, double *unused, double *out_8236083932693127920) {
+  h_26(state, unused, out_8236083932693127920);
 }
-void car_H_26(double *state, double *unused, double *out_5695262987630304857) {
-  H_26(state, unused, out_5695262987630304857);
+void car_H_26(double *state, double *unused, double *out_6605467162499159956) {
+  H_26(state, unused, out_6605467162499159956);
 }
-void car_h_27(double *state, double *unused, double *out_4062829090708927026) {
-  h_27(state, unused, out_4062829090708927026);
+void car_h_27(double *state, double *unused, double *out_5200052562052469124) {
+  h_27(state, unused, out_5200052562052469124);
 }
-void car_H_27(double *state, double *unused, double *out_4268050737513998691) {
-  H_27(state, unused, out_4268050737513998691);
+void car_H_27(double *state, double *unused, double *out_1877963186066088112) {
+  H_27(state, unused, out_1877963186066088112);
 }
-void car_h_29(double *state, double *unused, double *out_7564412822171847455) {
-  h_29(state, unused, out_7564412822171847455);
+void car_h_29(double *state, double *unused, double *out_1767262602628563370) {
+  h_29(state, unused, out_1767262602628563370);
 }
-void car_H_29(double *state, double *unused, double *out_5981413464383549724) {
-  H_29(state, unused, out_5981413464383549724);
+void car_H_29(double *state, double *unused, double *out_164600459196537079) {
+  H_29(state, unused, out_164600459196537079);
 }
-void car_h_28(double *state, double *unused, double *out_1579900780372029809) {
-  h_28(state, unused, out_1579900780372029809);
+void car_h_28(double *state, double *unused, double *out_8284311513975789260) {
+  h_28(state, unused, out_8284311513975789260);
 }
-void car_H_28(double *state, double *unused, double *out_7382931592256471318) {
-  H_28(state, unused, out_7382931592256471318);
+void car_H_28(double *state, double *unused, double *out_4917798557872993495) {
+  H_28(state, unused, out_4917798557872993495);
 }
-void car_h_31(double *state, double *unused, double *out_6613551970606485049) {
-  h_31(state, unused, out_6613551970606485049);
+void car_h_31(double *state, double *unused, double *out_588698570962583700) {
+  h_31(state, unused, out_588698570962583700);
 }
-void car_H_31(double *state, double *unused, double *out_8979331805328230107) {
-  H_31(state, unused, out_8979331805328230107);
+void car_H_31(double *state, double *unused, double *out_7231675264732511432) {
+  H_31(state, unused, out_7231675264732511432);
 }
 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_555027713999943867);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_7816877518147233589);
-void car_H_mod_fun(double *state, double *out_4533074481319696241);
-void car_f_fun(double *state, double dt, double *out_5039337670867030572);
-void car_F_fun(double *state, double dt, double *out_5181832378332727216);
-void car_h_25(double *state, double *unused, double *out_9064502358369772671);
-void car_H_25(double *state, double *unused, double *out_9009977767205190535);
-void car_h_24(double *state, double *unused, double *out_4315668618686905201);
-void car_H_24(double *state, double *unused, double *out_4579303311377586733);
-void car_h_30(double *state, double *unused, double *out_8507922675073509890);
-void car_H_30(double *state, double *unused, double *out_2093287425713573780);
-void car_h_26(double *state, double *unused, double *out_4180836484642303770);
-void car_H_26(double *state, double *unused, double *out_5695262987630304857);
-void car_h_27(double *state, double *unused, double *out_4062829090708927026);
-void car_H_27(double *state, double *unused, double *out_4268050737513998691);
-void car_h_29(double *state, double *unused, double *out_7564412822171847455);
-void car_H_29(double *state, double *unused, double *out_5981413464383549724);
-void car_h_28(double *state, double *unused, double *out_1579900780372029809);
-void car_H_28(double *state, double *unused, double *out_7382931592256471318);
-void car_h_31(double *state, double *unused, double *out_6613551970606485049);
-void car_H_31(double *state, double *unused, double *out_8979331805328230107);
+void car_err_fun(double *nom_x, double *delta_x, double *out_3745689373699638561);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_2400897459040094715);
+void car_H_mod_fun(double *state, double *out_7767655668809768572);
+void car_f_fun(double *state, double dt, double *out_3612821593330881774);
+void car_F_fun(double *state, double dt, double *out_2179834667538047746);
+void car_h_25(double *state, double *unused, double *out_6182136655387767236);
+void car_H_25(double *state, double *unused, double *out_2863963843625103732);
+void car_h_24(double *state, double *unused, double *out_7699556082354746130);
+void car_H_24(double *state, double *unused, double *out_8991184345786785929);
+void car_h_30(double *state, double *unused, double *out_3234410665032282275);
+void car_H_30(double *state, double *unused, double *out_345630885117855105);
+void car_h_26(double *state, double *unused, double *out_8236083932693127920);
+void car_H_26(double *state, double *unused, double *out_6605467162499159956);
+void car_h_27(double *state, double *unused, double *out_5200052562052469124);
+void car_H_27(double *state, double *unused, double *out_1877963186066088112);
+void car_h_29(double *state, double *unused, double *out_1767262602628563370);
+void car_H_29(double *state, double *unused, double *out_164600459196537079);
+void car_h_28(double *state, double *unused, double *out_8284311513975789260);
+void car_H_28(double *state, double *unused, double *out_4917798557872993495);
+void car_h_31(double *state, double *unused, double *out_588698570962583700);
+void car_H_31(double *state, double *unused, double *out_7231675264732511432);
 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_7828077856483360191);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_978622641958652250);
-void pose_H_mod_fun(double *state, double *out_7441553898687288237);
-void pose_f_fun(double *state, double dt, double *out_8648120814934185415);
-void pose_F_fun(double *state, double dt, double *out_5073381682457521558);
-void pose_h_4(double *state, double *unused, double *out_2401275546657747080);
-void pose_H_4(double *state, double *unused, double *out_1593472091935329516);
-void pose_h_10(double *state, double *unused, double *out_7020895229304053766);
-void pose_H_10(double *state, double *unused, double *out_299233475658510513);
-void pose_h_13(double *state, double *unused, double *out_2810480351238834931);
-void pose_H_13(double *state, double *unused, double *out_4805745917267662317);
-void pose_h_14(double *state, double *unused, double *out_6318162405249945056);
-void pose_H_14(double *state, double *unused, double *out_5556712948274814045);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_5813097801788606117);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_265932847343752274);
+void pose_H_mod_fun(double *state, double *out_3782749078324294962);
+void pose_f_fun(double *state, double dt, double *out_2316687695349101514);
+void pose_F_fun(double *state, double dt, double *out_830203783768276833);
+void pose_h_4(double *state, double *unused, double *out_1652667498977980545);
+void pose_H_4(double *state, double *unused, double *out_2065332728427663759);
+void pose_h_10(double *state, double *unused, double *out_8803253396773735652);
+void pose_H_10(double *state, double *unused, double *out_1768777245936477788);
+void pose_h_13(double *state, double *unused, double *out_5388839778110327076);
+void pose_H_13(double *state, double *unused, double *out_1146941096904669042);
+void pose_h_14(double *state, double *unused, double *out_653899258242143155);
+void pose_H_14(double *state, double *unused, double *out_1897908127911820770);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

selfdrive/modeld/modeld.py

@@ -66,6 +66,12 @@ def _model_smooth_seconds(params, key, default):
     return default
   value = params.get_float(key, return_default=True, default=default)
   return round(min(max(value, SMOOTH_SECONDS_STEP), 2.0) / SMOOTH_SECONDS_STEP) * SMOOTH_SECONDS_STEP
+
+
+def _should_publish_model_output(model_output, vipc_dropped_frames: int) -> bool:
+  return model_output is not None and vipc_dropped_frames == 0
+
+
 MIN_LAT_CONTROL_SPEED = 0.3
 BIG_MODEL_LOAD_WAIT_TIMEOUT_MS = 30000
 BIG_MODEL_RUN_WAIT_TIMEOUT_MS = 3000
@@ -933,7 +939,10 @@ def main(demo=False):
     mt2 = time.perf_counter()
     model_execution_time = mt2 - mt1
 
-    if model_output is not None:
+    if model_output is not None and vipc_dropped_frames > 0:
+      cloudlog.error(f"suppressing model output after dropping {vipc_dropped_frames} frames")
+
+    if _should_publish_model_output(model_output, vipc_dropped_frames):
       modelv2_send = messaging.new_message('modelV2')
       starpilot_modelv2_send = messaging.new_message('starpilotModelV2')
       drivingdata_send = messaging.new_message('drivingModelData')