Build

opendbc_repo/opendbc/car/hyundai/carstate.py

@@ -130,6 +130,8 @@ class CarState(CarStateBase):
     self.ccnc_162_ts_ns = 0
     self.ccnc_1b5_ts_ns = 0
     self.dashboard_limit = LimitTracker()
+    self.dashboard_stop_sign = None
+    self.drive_mode = None
     self.ioniq6_bsm_sources = (Ioniq6BlindspotSources()
                                if CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and CP.openpilotLongitudinalControl else None)
     self.ioniq6_bsm_now_ns = 0
@@ -604,6 +606,13 @@ class CarState(CarStateBase):
       status, value_mps = hyundai_canfd_sign(speed, system_status, self.is_metric)
       self.dashboard_limit.update(timestamp, status, value_mps, valid_until_ns=expiry)
 
+    stamp = cp_cam.ts_nanos["ADAS_0x380"]["STOP_SIGN"]
+    self.dashboard_stop_sign = (bool(cp_cam.vl["ADAS_0x380"]["STOP_SIGN"]), stamp) if stamp > 0 else None
+
+    if self.CP.flags & HyundaiFlags.EV:
+      stamp = cp.ts_nanos["DRIVE_MODE_EV"]["DRIVE_MODE"]
+      self.drive_mode = (int(cp.vl["DRIVE_MODE_EV"]["DRIVE_MODE"]), stamp) if stamp > 0 else None
+
     return ret
 
   def get_can_parsers_canfd(self, CP):
@@ -618,6 +627,7 @@ class CarState(CarStateBase):
       msgs.extend((("ADAS_CMD_50_50ms", math.nan), ("BLINDSPOTS_FRONT_CORNER_2", math.nan)))
     if CP.flags & HyundaiFlags.EV:
       msgs.append(("MANUAL_SPEED_LIMIT_ASSIST", math.nan))
+      msgs.append(("DRIVE_MODE_EV", math.nan))
     sign_subscription = ("FR_CMR_02_100ms", math.nan)
     if CP.flags & HyundaiFlags.CANFD_LKA_STEER_MSG:
       msgs.append(sign_subscription)
@@ -699,6 +709,7 @@ class CarState(CarStateBase):
         msgs.append(("SCC_CONTROL", 50))
       if ("CAM_0x2a4", 20) not in cam_msgs:
         cam_msgs.append(("CAM_0x2a4", 20))
+    cam_msgs.append(("ADAS_0x380", math.nan))
     return {
       Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], msgs, CanBus(CP).ECAN),
       # Native CANParser accepts NaN to subscribe without a CAN-valid frequency gate.

opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc

@@ -594,6 +594,11 @@ BO_ 512 ADRV_0x200: 8 ADRV
  SG_ SET_ME_E1 : 24|8@1+ (1,0) [0|255] "" XXX
  SG_ SET_ME_3A : 32|8@1+ (1,0) [0|255] "" XXX
 
+BO_ 517 DRIVE_MODE_EV: 8 XXX
+ SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
+ SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
+ SG_ DRIVE_MODE : 24|8@1+ (1,0) [0|255] "" XXX
+
 BO_ 593 RADAR_0x251: 16 FRONT_RADAR
  SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
  SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
@@ -695,6 +700,9 @@ BO_ 874 BLINDSPOTS_FRONT_CORNER_2: 16 XXX
  SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
  SG_ SIDE_DETECT_STATE : 24|8@1+ (1,0) [0|255] "" XXX
 
+BO_ 896 ADAS_0x380: 24 XXX
+ SG_ STOP_SIGN : 83|1@1+ (1,0) [0|1] "" XXX
+
 BO_ 961 BLINKER_STALKS: 8 XXX
  SG_ CHECKSUM_MAYBE : 7|8@0+ (1,0) [0|255] "" XXX
  SG_ COUNTER_ALT : 15|4@0+ (1,0) [0|15] "" XXX
@@ -1149,6 +1157,7 @@ VAL_ 506 ISLA_AutoUsmSta 0 "None Auto Function (Delete Menu)" 1 "Auto Off" 2 "Au
 VAL_ 506 ISLA_Cntry 0 "Europe/Russia/Australia" 1 "Domestic" 2 "China" 3 "USA" 4 "Canada" 5 "Australia" 6 "Reserved" 7 "Reserved" 8 "Reserved" 9 "Reserved" 10 "Reserved" 11 "Reserved" 12 "Reserved" 13 "Reserved" 14 "Reserved" 15 "Initial Value (default)";
 VAL_ 506 ISLA_AddtnlSign 0 "No Recognition (default)" 1 "School Crossing" 16 "Do Not Pass" 17 "Reserved" 18 "Reserved" 19 "Reserved" 20 "Reserved" 21 "Reserved" 22 "Reserved" 23 "Reserved" 24 "Exit" 25 "Roundabout" 26 "Right Curve" 27 "Left Curve" 28 "Winding Road" 29 "Reserved" 30 "Reserved" 31 "Reserved" 2 "Pedestrian Crossing" 3 "Bicycle Crossing" 4 "Reserved" 5 "Reserved" 6 "Reserved" 7 "Reserved" 8 "Stop" 9 "Yield" 10 "Stop Ahead" 11 "Yield Ahead" 12 "Road Construction Ahead" 13 "Lane Reduction" 14 "Reserved" 15 "Reserved";
 VAL_ 506 ISLA_SchoolZone 0 "No School Zone" 1 "School Zone" 2 "Reserved" 3 "Reserved";
+VAL_ 517 DRIVE_MODE 2 "Normal" 4 "Eco" 5 "Sport";
 VAL_ 687 HOD_Dir_Status 0 "HANDS OFF" 1 "HAND TOUCH (SOFT)" 2 "HAND TOUCH (STRONG)" 3 "HAND GRIP (SOFT)" 4 "HAND GRIP (STRONG)" 5 "RESERVED" 6 "RESERVED";
 VAL_ 698 IFSref_FR_CMR_Sta 0 "None Option (Default)" 1 "Normal" 2 "Blockage Status" 3 "Error Indicator";
 VAL_ 698 IFSref_VehNumVal 0 "No vehicle" 1 "Number of vehicles" 2 "Number of vehicles" 3 "Number of vehicles" 4 "Number of vehicles" 5 "Number of vehicles" 6 "Number of vehicles" 7 "Number of vehicles" 8 "Number of vehicles" 9 "Number of vehicles" 10 "Number of vehicles" 11 "Over than 10 vehicles" 12 "Reserved" 13 "Reserved" 14 "Default" 15 "Error indicator";

openpilot/common/params_keys.h

@@ -74,7 +74,11 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"QOLLongitudinal", {PERSISTENT, BOOL}},
     {"ReverseCruise", {PERSISTENT, BOOL, "0"}},
     {"ForceStops", {PERSISTENT, BOOL, "0"}},
+    {"DriveModeProfiles", {PERSISTENT, BOOL, "0"}},
+    {"DriveModeAcceleration", {PERSISTENT, BOOL, "1"}},
+    {"DriveModeDeceleration", {PERSISTENT, BOOL, "1"}},
     {"AlwaysAllowUploads", {PERSISTENT, BOOL, "0"}},
+    {"AggressiveCoolingEnabled", {PERSISTENT, BOOL, "0"}},
     {"UsageStatsState", {PERSISTENT | DONT_LOG, JSON}},
     {"UsageStatsStatus", {CLEAR_ON_MANAGER_START | DONT_LOG, JSON}},
     {"TeslaAOLScreenTap", {PERSISTENT, BOOL, "0"}},
@@ -193,6 +197,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"PauseLateralOnSignal", {PERSISTENT, BOOL, "0"}},
     {"LateralResumeDelay", {PERSISTENT, FLOAT, "0.0"}},
     {"TurnAssist", {PERSISTENT, BOOL, "1"}},
+    {"HighwaySmoothing", {PERSISTENT, BOOL, "0"}},
+    {"HighwayCorrectionGain", {PERSISTENT, FLOAT, "0.3"}},
     {"GMPedalLongitudinal", {PERSISTENT, BOOL, "0"}},
     {"ApiCache_Device", {PERSISTENT, STRING}},
     {"ApiCache_FirehoseStats", {PERSISTENT, JSON}},

openpilot/selfdrive/car/card.py

@@ -1122,6 +1122,9 @@ class Car:
     if self.CP.brand == 'mazda':
       from openpilot.starpilot.car.mazda.dashboard import compose
       companion = compose(self.CP, self.CI.CS, CS, companion, car_state_stamp=int(cs_send.logMonoTime))
+    elif self.CP.brand == 'hyundai':
+      from openpilot.starpilot.car.hyundai.dashboard import compose as hyundai_compose
+      companion = hyundai_compose(self.CP, self.CI.CS, CS, companion, car_state_stamp=int(cs_send.logMonoTime))
     if self.v_cruise_helper.toyota is not None:
       if companion is None:
         companion = messaging.new_message('starpilotCarState', valid=bool(cs_send.valid), logMonoTime=int(cs_send.logMonoTime))

openpilot/selfdrive/controls/controlsd.py

@@ -18,6 +18,7 @@ from openpilot.common.swaglog import cloudlog
 from opendbc.car.car_helpers import interfaces
 from opendbc.car.vehicle_model import VehicleModel
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from openpilot.selfdrive.controls.lib.highway_correction_gain import HighwayCorrectionGain, predicted_lateral_accels, read_gain
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_angle import STEER_ANGLE_SATURATION_THRESHOLD
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
@@ -101,6 +102,8 @@ class Controls:
     self.model_turn_assist = ModelTurnAssist()
     self.lane_centering_controller = (LaneCenteringController(body=interface_dimensions(self.CI, self.CP), body_required=True)
                                       if predict_lane_clearance else LaneCenteringController())
+    self.highway_correction_gain = HighwayCorrectionGain()
+    self.highway_gain = read_gain(self.params)
     self.lateral_pause = LateralPause(read_lateral_pause(self.params), self.params)
     self.lane_change_preferences = LiveLaneChangePolicy(self.params,
       now_ns=0 if os.getenv('REPLAY') == '1' else time.monotonic_ns())
@@ -381,6 +384,17 @@ class Controls:
     elif getattr(self, 'model_turn_assist', None) is not None:
       self.model_turn_assist.reset()
       self.model_turn_assist.twitch_guard_remaining = update_twitch_guard(0., CS.vEgo, CS.standstill)
+    if self.sm.frame % 100 == 0:
+      self.highway_gain = read_gain(self.params)
+    predicted = None
+    if self.sm.all_checks(['modelV2']):
+      predicted = predicted_lateral_accels(model_v2.position.t, model_v2.velocity.x, model_v2.orientationRate.z)
+    # before lane centering so its guard measures only its own correction against the smoothed base
+    new_desired_curvature = self.highway_correction_gain.update(
+      new_desired_curvature, CS.vEgo, CC.latActive, self.highway_gain,
+      bypass=bool(CS.leftBlinker or CS.rightBlinker or CS.steeringPressed or
+                  model_v2.meta.laneChangeState != LaneChangeState.off or self.sm.valid['lateralManeuverPlan']),
+      predicted=predicted)
     lane_base_curvature = new_desired_curvature
     self.lane_centering_applied = 0.0
     self.lane_centering_inactive_reason = 'inactive'

openpilot/selfdrive/controls/lib/highway_correction_gain.py

@@ -0,0 +1,127 @@
+import math
+from collections import deque
+
+import numpy as np
+
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_CTRL
+from openpilot.starpilot.saved_source import read_saved
+
+BASELINE_TAU = 1.5                        # s
+SPEED_OFF = 30.0 * CV.MPH_TO_MS
+SPEED_ON = 40.0 * CV.MPH_TO_MS
+CURVE_LEVEL_TAU = 3.0                     # s
+LAT_ACCEL_ON = 0.25                       # m/s^2
+LAT_ACCEL_OFF = 0.6                       # m/s^2
+CURVE_LEVEL_BLEND = (0.15, 0.35)          # m/s^2
+PREDICTION_HORIZONS = (1.5, 2.0)          # s
+PREDICTED_CURVE_ON = 0.20                 # m/s^2
+PREDICTED_CURVE_OFF = 0.45                # m/s^2
+CURVE_DEVIATION_ON = 0.12                 # m/s^2
+CURVE_DEVIATION_OFF = 0.30                # m/s^2
+TIGHT_CURVE_ON = 1.5                      # m/s^2
+TIGHT_CURVE_OFF = 2.0                     # m/s^2
+# held longer than the weave's peak spacing so the weave can't modulate its own gain
+ENVELOPE_HOLD = 2.0                       # s
+ENVELOPE_RELEASE_RATE = 0.3               # m/s^2 per second
+BYPASS_FADE_RATE = 2.0                    # per second
+MIN_GAIN = 0.1
+DEFAULT_GAIN = 0.3
+ENABLE_KEY = "HighwaySmoothing"
+GAIN_KEY = "HighwayCorrectionGain"
+
+
+def _smoothstep(x: float, lo: float, hi: float) -> float:
+  t = min(max((x - lo) / (hi - lo), 0.0), 1.0)
+  return t * t * (3.0 - 2.0 * t)
+
+
+def read_gain(params) -> float:
+  enabled, readable = read_saved(params, ENABLE_KEY, 8)
+  if not readable or enabled != b"1":
+    return 1.0
+  raw, readable = read_saved(params, GAIN_KEY, 16)
+  try:
+    gain = DEFAULT_GAIN if raw is None else float(raw) if readable else 1.0
+  except ValueError:
+    return 1.0
+  return min(max(gain, MIN_GAIN), 1.0) if math.isfinite(gain) else 1.0
+
+
+def predicted_lateral_accels(t, velocity_x, yaw_rate) -> tuple[float, ...] | None:
+  t, velocity_x, yaw_rate = (np.asarray(list(x), dtype=float) for x in (t, velocity_x, yaw_rate))
+  if len(t) < 2 or len(t) != len(velocity_x) or len(t) != len(yaw_rate) or t[-1] < PREDICTION_HORIZONS[-1]:
+    return None
+  return tuple(float(np.interp(h, t, velocity_x) * np.interp(h, t, yaw_rate)) for h in PREDICTION_HORIZONS)
+
+
+class _Envelope:
+  def __init__(self):
+    self.frame = 0
+    self.peaks: deque[tuple[int, float]] = deque()
+    self.value = 0.0
+
+  def update(self, x: float) -> float:
+    self.frame += 1
+    while self.peaks and self.peaks[-1][1] <= x:
+      self.peaks.pop()
+    self.peaks.append((self.frame, x))
+    while self.peaks[0][0] <= self.frame - ENVELOPE_HOLD / DT_CTRL:
+      self.peaks.popleft()
+    self.value = max(self.peaks[0][1], self.value - ENVELOPE_RELEASE_RATE * DT_CTRL)
+    return self.value
+
+
+class HighwayCorrectionGain:
+  def __init__(self):
+    self.alpha = DT_CTRL / (BASELINE_TAU + DT_CTRL)
+    self.alpha_curve = DT_CTRL / (CURVE_LEVEL_TAU + DT_CTRL)
+    self.reset()
+
+  def reset(self, curvature: float = 0.0) -> None:
+    self.baseline = curvature
+    self.curve_level = curvature
+    self.envelope = _Envelope()
+    self.prediction_envelope = _Envelope()
+    self.bypass_weight = 0.0
+    self.weight = 0.0
+
+  def update(self, curvature: float, v_ego: float, lat_active: bool, gain: float, bypass: bool,
+             predicted: tuple[float, ...] | None = None) -> float:
+    if not lat_active:
+      self.reset(curvature)
+      return curvature
+
+    # tracked even when bypassed so fading in never steps the output
+    self.baseline += self.alpha * (curvature - self.baseline)
+    self.curve_level += self.alpha_curve * (curvature - self.curve_level)
+
+    v2 = v_ego ** 2
+    level = abs(self.curve_level) * v2
+    in_curve = _smoothstep(level, *CURVE_LEVEL_BLEND)
+    reference = in_curve * self.curve_level
+    envelope = self.envelope.update(abs(curvature - reference) * v2)
+
+    if predicted:
+      gate = 1.0 - _smoothstep(envelope, LAT_ACCEL_ON, LAT_ACCEL_OFF)
+      deviation = max(abs(p - reference * v2) for p in predicted)
+      on = LAT_ACCEL_ON + in_curve * (PREDICTED_CURVE_ON - LAT_ACCEL_ON)
+      off = LAT_ACCEL_OFF + in_curve * (PREDICTED_CURVE_OFF - LAT_ACCEL_OFF)
+      gate *= 1.0 - _smoothstep(self.prediction_envelope.update(deviation), on, off)
+    else:
+      if self.prediction_envelope.frame:
+        self.prediction_envelope = _Envelope()
+      on = LAT_ACCEL_ON + in_curve * (CURVE_DEVIATION_ON - LAT_ACCEL_ON)
+      off = LAT_ACCEL_OFF + in_curve * (CURVE_DEVIATION_OFF - LAT_ACCEL_OFF)
+      gate = 1.0 - _smoothstep(envelope, on, off)
+    gate *= 1.0 - _smoothstep(level, TIGHT_CURVE_ON, TIGHT_CURVE_OFF)
+
+    step = BYPASS_FADE_RATE * DT_CTRL
+    self.bypass_weight += min(max((0.0 if bypass else 1.0) - self.bypass_weight, -step), step)
+
+    gain = min(max(gain, MIN_GAIN), 1.0)
+    self.weight = self.bypass_weight * _smoothstep(v_ego, SPEED_OFF, SPEED_ON) * gate
+    k = 1.0 - self.weight * (1.0 - gain)
+    if k >= 1.0:
+      return curvature
+    return self.baseline + k * (curvature - self.baseline)

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json

@@ -1,7 +1,7 @@
 {
-    "acados_include_path": "/work/.venv-linux-arm64/lib/python3.12/site-packages/acados/install/include",
-    "acados_lib_path": "/work/.venv-linux-arm64/lib/python3.12/site-packages/acados/install/lib",
-    "code_export_directory": "/work/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code",
+    "acados_include_path": "/usr/local/venv/lib/python3.12/site-packages/acados/install/include",
+    "acados_lib_path": "/usr/local/venv/lib/python3.12/site-packages/acados/install/lib",
+    "code_export_directory": "/data/build_bluescreensonly/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code",
     "constraints": {
         "C": [],
         "C_e": [],
@@ -351,8 +351,8 @@
         ]
     },
     "cython_include_dirs": [
-        "/work/.venv-linux-arm64/lib/python3.12/site-packages/numpy/_core/include",
-        "/work/.cache/uv-python/cpython-3.12.13-linux-aarch64-gnu/include/python3.12"
+        "/usr/local/venv/lib/python3.12/site-packages/numpy/_core/include",
+        "/usr/include/python3.12"
     ],
     "dims": {
         "N": 12,
@@ -388,7 +388,7 @@
         "ny_e": 5,
         "nz": 0
     },
-    "json_file": "/work/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
+    "json_file": "/data/build_bluescreensonly/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
     "model": {
         "con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
         "con_h_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/Makefile

@@ -83,8 +83,8 @@ OBJ+= $(OCP_OBJ)
 EXTERNAL_DIR=
 EXTERNAL_LIB=
 
-INCLUDE_PATH = /work/.venv-linux-arm64/lib/python3.12/site-packages/acados/install/include
-LIB_PATH = /work/.venv-linux-arm64/lib/python3.12/site-packages/acados/install/lib
+INCLUDE_PATH = /usr/local/venv/lib/python3.12/site-packages/acados/install/include
+LIB_PATH = /usr/local/venv/lib/python3.12/site-packages/acados/install/lib
 
 # preprocessor flags for make's implicit rules
 CPPFLAGS+= -I$(INCLUDE_PATH)
@@ -145,7 +145,7 @@ ocp_cython_c: ocp_shared_lib
 	-o acados_ocp_solver_pyx.c \
 	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
 	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
-	-I /work/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
+	-I /data/build_bluescreensonly/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
 
 ocp_cython_o: ocp_cython_c
 	$(CC) $(ACADOS_FLAGS) -c -O2 \
@@ -154,8 +154,8 @@ ocp_cython_o: ocp_cython_c
 	-I $(INCLUDE_PATH)/blasfeo/include/ \
 	-I $(INCLUDE_PATH)/hpipm/include/ \
 	-I $(INCLUDE_PATH) \
-	-I /work/.venv-linux-arm64/lib/python3.12/site-packages/numpy/_core/include \
-	-I /work/.cache/uv-python/cpython-3.12.13-linux-aarch64-gnu/include/python3.12 \
+	-I /usr/local/venv/lib/python3.12/site-packages/numpy/_core/include \
+	-I /usr/include/python3.12 \
 	acados_ocp_solver_pyx.c \
 
 ocp_cython: ocp_cython_o
@@ -172,7 +172,7 @@ sim_cython_c: sim_shared_lib
 	-o acados_sim_solver_pyx.c \
 	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
 	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
-	-I /work/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
+	-I /data/build_bluescreensonly/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
 
 sim_cython_o: sim_cython_c
 	$(CC) $(ACADOS_FLAGS) -c -O2 \
@@ -181,8 +181,8 @@ sim_cython_o: sim_cython_c
 	-I $(INCLUDE_PATH)/blasfeo/include/ \
 	-I $(INCLUDE_PATH)/hpipm/include/ \
 	-I $(INCLUDE_PATH) \
-	-I /work/.venv-linux-arm64/lib/python3.12/site-packages/numpy/_core/include \
-	-I /work/.cache/uv-python/cpython-3.12.13-linux-aarch64-gnu/include/python3.12 \
+	-I /usr/local/venv/lib/python3.12/site-packages/numpy/_core/include \
+	-I /usr/include/python3.12 \
 	acados_sim_solver_pyx.c \
 
 sim_cython: sim_cython_o

openpilot/selfdrive/controls/plannerd.py

@@ -15,6 +15,7 @@ from openpilot.cereal.services import SERVICE_LIST
 from openpilot.starpilot.speed_limits import history as slc_history
 from openpilot.starpilot.speed_limits.runtime import Action as SlcAction, CruiseEvent as SlcCruiseEvent, Runtime as SlcRuntime
 from openpilot.starpilot.longitudinal.profile_runtime import ProfileHost
+from openpilot.starpilot.longitudinal.drive_mode import observe as observe_drive_mode
 from openpilot.starpilot.longitudinal.lane_change_gap import LaneGapPreferences
 from openpilot.starpilot.longitudinal.experimental_release import ConditionalHandoff
 from openpilot.starpilot.longitudinal.lead_approach import LeadApproachKey
@@ -442,6 +443,8 @@ def starpilot_main():
   optional_slc = ['slcDashboardObservation', 'slcVisionObservation', 'mapdOut', 'starpilotCarState'] if slc_available else []
   if slc_available and feature_enabled(params, CP, 'aol', os.environ):
     optional_slc.append('aolAxisState')
+  if force_stop_owner is not None and 'starpilotCarState' not in optional_slc:
+    optional_slc.append('starpilotCarState')
   subscribed_services.extend(optional_slc)
   subscribed_services.append('starpilotNavigation')
   optional_conditional = ['starpilotNavigation'] + (['starpilotRadarState'] if conditional_host is not None else []) + optional_slc
@@ -589,7 +592,8 @@ def starpilot_main():
         # selected response supersedes the old global-only channel.
         global_braking_response = None
         selected_profiles = (profile_host.sample_selected(now_ns, sm['selfdriveState'].personality,
-                              sm['carState'].vEgo, CP, traffic_mode=traffic_mode, legacy=profile_tuning)
+                              sm['carState'].vEgo, CP, traffic_mode=traffic_mode, legacy=profile_tuning,
+                              drive_mode=observe_drive_mode(sm, now_ns=now_ns))
                              if profile_replay and profile_host is not None and
                              _profile_frame_valid(sm, now_ns) else None)
         lane_change_policy = lane_gap_preferences.sample(now_ns) if lane_gap_preferences is not None else None

openpilot/selfdrive/controls/tests/test_highway_correction_gain.py

@@ -0,0 +1,198 @@
+import math
+
+import numpy as np
+
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.highway_correction_gain import HighwayCorrectionGain, predicted_lateral_accels
+
+
+_V_HWY = 31.0
+
+
+def _run(hcg, curvature, gain=0.7, v_ego=_V_HWY, lat_active=True, bypass=None, predicted=None):
+  bypass = np.zeros(len(curvature), bool) if bypass is None else bypass
+  predicted = [None] * len(curvature) if predicted is None else [tuple(p) for p in predicted]
+  return np.array([hcg.update(float(c), v_ego, lat_active, gain, bool(b), predicted=p)
+                   for c, b, p in zip(curvature, bypass, predicted, strict=True)])
+
+
+def _lead(lat, seconds):
+  n = int(seconds / DT_CTRL)
+  ahead = np.concatenate([lat[n:], np.full(n, lat[-1])])
+  return np.c_[ahead, ahead]
+
+
+def _weave(seconds=30.0, freq=0.6, lat_accel_amp=0.08, v_ego=_V_HWY, bias=0.0):
+  t = np.arange(0.0, seconds, DT_CTRL)
+  return bias + (lat_accel_amp / v_ego ** 2) * np.sin(2 * math.pi * freq * t)
+
+
+def _fit_sine(x, freq):
+  t = np.arange(len(x)) * DT_CTRL
+  basis = np.c_[np.sin(2 * math.pi * freq * t), np.cos(2 * math.pi * freq * t), np.ones(len(x))]
+  (s, c, _), *_ = np.linalg.lstsq(basis, x, rcond=None)
+  return math.hypot(s, c), math.atan2(c, s)
+
+
+def test_gain_one_is_exact_pass_through():
+  raw = _weave(bias=0.1 / _V_HWY ** 2)
+  np.testing.assert_array_equal(_run(HighwayCorrectionGain(), raw, gain=1.0), raw)
+
+
+def test_weave_band_scaled_by_gain_with_little_lag():
+  for freq in (0.4, 0.6, 0.8):
+    raw = _weave(freq=freq)
+    out = _run(HighwayCorrectionGain(), raw, gain=0.7)
+    tail = slice(len(raw) // 2, None)
+    amp_in, ph_in = _fit_sine(raw[tail], freq)
+    amp_out, ph_out = _fit_sine(out[tail], freq)
+    assert 0.68 < amp_out / amp_in < 0.76, freq
+    lag_deg = math.degrees((ph_in - ph_out + math.pi) % (2 * math.pi) - math.pi)
+    assert 0.0 <= lag_deg < 8.0, (freq, lag_deg)
+
+
+def test_steady_offset_passes_unchanged():
+  bias = 0.15 / _V_HWY ** 2
+  raw = np.full(int(30 / DT_CTRL), bias)
+  out = _run(HighwayCorrectionGain(), raw, gain=0.5)
+  np.testing.assert_allclose(out[-100:], bias, rtol=1e-6)
+
+
+def test_steady_curve_wobble_is_smoothed():
+  raw = _weave(seconds=40.0, bias=1.0 / _V_HWY ** 2)
+  out = _run(HighwayCorrectionGain(), raw, gain=0.5)
+  tail = slice(len(raw) // 2, None)
+  amp_in, _ = _fit_sine(raw[tail], 0.6)
+  amp_out, _ = _fit_sine(out[tail], 0.6)
+  assert 0.45 < amp_out / amp_in < 0.62
+  assert abs(out[tail].mean() - raw[tail].mean()) * _V_HWY ** 2 < 0.01
+
+
+def test_tight_curves_pass_through():
+  raw = _weave(seconds=40.0, bias=2.5 / _V_HWY ** 2)
+  out = _run(HighwayCorrectionGain(), raw, gain=0.5)
+  tail = slice(len(raw) // 2, None)
+  np.testing.assert_allclose(out[tail], raw[tail], rtol=0, atol=1e-12)
+
+
+def test_curve_entry_is_not_delayed_much():
+  t = np.arange(0.0, 20.0, DT_CTRL)
+  lat = np.interp(t, [0, 5, 7, 20], [0, 0, 1.2, 1.2])
+  raw = lat / _V_HWY ** 2
+  out = _run(HighwayCorrectionGain(), raw, gain=0.5)
+  shortfall = (raw - out) * _V_HWY ** 2
+  # the gate only fully releases at LAT_ACCEL_OFF, so curve entry is briefly softened
+  assert np.max(np.abs(shortfall)) < 0.15
+  assert np.argmax(out * _V_HWY ** 2 >= 0.6) == np.argmax(lat >= 0.6)
+  assert np.max(np.abs(shortfall[t > 12.0])) < 0.01
+
+
+def test_curve_exit_is_not_held():
+  t = np.arange(0.0, 30.0, DT_CTRL)
+  lat = np.interp(t, [0, 2, 20, 22, 30], [1.2, 1.2, 1.2, 0, 0])
+  raw = lat / _V_HWY ** 2
+  hcg = HighwayCorrectionGain()
+  hcg.reset(raw[0])
+  out = _run(hcg, raw, gain=0.5)
+  excess = (out - raw)[t > 19.0] * _V_HWY ** 2
+  assert np.max(np.abs(excess)) < 0.15
+  i_raw = np.argmax(lat[t > 19.0] <= 0.6)
+  i_out = np.argmax(out[t > 19.0] * _V_HWY ** 2 <= 0.6)
+  assert (i_out - i_raw) * DT_CTRL < 0.15
+
+
+def test_below_speed_gate_passes_through():
+  v = 13.0
+  raw = _weave(v_ego=v)
+  np.testing.assert_allclose(_run(HighwayCorrectionGain(), raw, gain=0.5, v_ego=v), raw, rtol=0, atol=1e-12)
+
+
+def test_bypass_fades_without_steps():
+  raw = _weave(seconds=30.0)
+  bypass = np.zeros(len(raw), bool)
+  bypass[int(15 / DT_CTRL):int(20 / DT_CTRL)] = True
+  hcg = HighwayCorrectionGain()
+  out = _run(hcg, raw, gain=0.5, bypass=bypass)
+  mid = slice(int(17 / DT_CTRL), int(20 / DT_CTRL))
+  np.testing.assert_allclose(out[mid], raw[mid], rtol=0, atol=1e-12)
+  max_raw_step = np.max(np.abs(np.diff(raw)))
+  assert np.max(np.abs(np.diff(out))) < 1.5 * max_raw_step
+
+
+def test_active_from_40_mph():
+  v = 40.0 * 0.44704
+  raw = _weave(v_ego=v)
+  out = _run(HighwayCorrectionGain(), raw, gain=0.5, v_ego=v)
+  tail = slice(len(raw) // 2, None)
+  amp_in, _ = _fit_sine(raw[tail], 0.6)
+  amp_out, _ = _fit_sine(out[tail], 0.6)
+  assert 0.48 < amp_out / amp_in < 0.56
+
+
+def test_inactive_resets_and_passes_through():
+  hcg = HighwayCorrectionGain()
+  raw = _weave(seconds=10.0)
+  _run(hcg, raw, gain=0.5)
+  assert hcg.weight > 0.9
+  out = _run(hcg, raw[:50], gain=0.5, lat_active=False)
+  np.testing.assert_array_equal(out, raw[:50])
+  assert hcg.weight == 0.0 and hcg.bypass_weight == 0.0
+
+
+def test_gain_is_clamped():
+  raw = _weave()
+  low = _run(HighwayCorrectionGain(), raw, gain=0.0)
+  floor = _run(HighwayCorrectionGain(), raw, gain=0.1)
+  np.testing.assert_allclose(low, floor, rtol=0, atol=1e-15)
+
+
+def test_predicted_lateral_accels():
+  t = 10.0 * (np.arange(33) / 32) ** 2
+  assert predicted_lateral_accels(t, np.full(33, 30.0), np.full(33, 0.02)) == (0.6, 0.6)
+  assert predicted_lateral_accels(t[:10], np.full(10, 30.0), np.full(10, 0.02)) is None
+  assert predicted_lateral_accels(t, np.full(32, 30.0), np.full(33, 0.02)) is None
+
+
+def test_prediction_smooths_wobbly_curve_more():
+  raw = _weave(seconds=40.0, lat_accel_amp=0.2, bias=1.0 / _V_HWY ** 2)
+  pred = np.full((len(raw), 2), 1.0)
+  tail = slice(len(raw) // 2, None)
+  amp_in, _ = _fit_sine(raw[tail], 0.6)
+  without = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.5)[tail], 0.6)[0] / amp_in
+  with_pred = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.5, predicted=pred)[tail], 0.6)[0] / amp_in
+  assert 0.45 < with_pred < 0.62
+  assert with_pred < without - 0.1
+
+
+def test_prediction_releases_before_curve_exit():
+  t = np.arange(0.0, 30.0, DT_CTRL)
+  lat = np.interp(t, [0, 20, 22, 30], [1.2, 1.2, 0, 0])
+  raw = lat / _V_HWY ** 2
+  def excess(predicted):
+    hcg = HighwayCorrectionGain(); hcg.reset(raw[0])
+    out = _run(hcg, raw, gain=0.5, predicted=predicted)
+    return np.max(np.abs((out - raw)[t > 19.0])) * _V_HWY ** 2
+  with_pred = excess(_lead(lat, 1.5))
+  assert with_pred < 0.02
+  assert with_pred < excess(None)
+
+
+def test_prediction_releases_before_curve_entry():
+  t = np.arange(0.0, 20.0, DT_CTRL)
+  lat = np.interp(t, [0, 5, 7, 20], [0, 0, 1.2, 1.2])
+  raw = lat / _V_HWY ** 2
+  def shortfall(predicted):
+    out = _run(HighwayCorrectionGain(), raw, gain=0.5, predicted=predicted)
+    return np.max(np.abs(raw - out)) * _V_HWY ** 2
+  with_pred = shortfall(_lead(lat, 1.5))
+  assert with_pred < 0.02
+  assert with_pred < shortfall(None)
+
+
+def test_straights_smooth_the_same_with_prediction():
+  raw = _weave()
+  pred = np.zeros((len(raw), 2))
+  tail = slice(len(raw) // 2, None)
+  a = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.7)[tail], 0.6)[0]
+  b = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.7, predicted=pred)[tail], 0.6)[0]
+  assert math.isclose(a, b, rel_tol=0.02)

openpilot/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_7353202191454035649) {
-   out_7353202191454035649[0] = delta_x[0] + nom_x[0];
-   out_7353202191454035649[1] = delta_x[1] + nom_x[1];
-   out_7353202191454035649[2] = delta_x[2] + nom_x[2];
-   out_7353202191454035649[3] = delta_x[3] + nom_x[3];
-   out_7353202191454035649[4] = delta_x[4] + nom_x[4];
-   out_7353202191454035649[5] = delta_x[5] + nom_x[5];
-   out_7353202191454035649[6] = delta_x[6] + nom_x[6];
-   out_7353202191454035649[7] = delta_x[7] + nom_x[7];
-   out_7353202191454035649[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_2869805078350318634) {
+   out_2869805078350318634[0] = delta_x[0] + nom_x[0];
+   out_2869805078350318634[1] = delta_x[1] + nom_x[1];
+   out_2869805078350318634[2] = delta_x[2] + nom_x[2];
+   out_2869805078350318634[3] = delta_x[3] + nom_x[3];
+   out_2869805078350318634[4] = delta_x[4] + nom_x[4];
+   out_2869805078350318634[5] = delta_x[5] + nom_x[5];
+   out_2869805078350318634[6] = delta_x[6] + nom_x[6];
+   out_2869805078350318634[7] = delta_x[7] + nom_x[7];
+   out_2869805078350318634[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_5273149315250963980) {
-   out_5273149315250963980[0] = -nom_x[0] + true_x[0];
-   out_5273149315250963980[1] = -nom_x[1] + true_x[1];
-   out_5273149315250963980[2] = -nom_x[2] + true_x[2];
-   out_5273149315250963980[3] = -nom_x[3] + true_x[3];
-   out_5273149315250963980[4] = -nom_x[4] + true_x[4];
-   out_5273149315250963980[5] = -nom_x[5] + true_x[5];
-   out_5273149315250963980[6] = -nom_x[6] + true_x[6];
-   out_5273149315250963980[7] = -nom_x[7] + true_x[7];
-   out_5273149315250963980[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_6624138795093603288) {
+   out_6624138795093603288[0] = -nom_x[0] + true_x[0];
+   out_6624138795093603288[1] = -nom_x[1] + true_x[1];
+   out_6624138795093603288[2] = -nom_x[2] + true_x[2];
+   out_6624138795093603288[3] = -nom_x[3] + true_x[3];
+   out_6624138795093603288[4] = -nom_x[4] + true_x[4];
+   out_6624138795093603288[5] = -nom_x[5] + true_x[5];
+   out_6624138795093603288[6] = -nom_x[6] + true_x[6];
+   out_6624138795093603288[7] = -nom_x[7] + true_x[7];
+   out_6624138795093603288[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_5463203357507594110) {
-   out_5463203357507594110[0] = 1.0;
-   out_5463203357507594110[1] = 0.0;
-   out_5463203357507594110[2] = 0.0;
-   out_5463203357507594110[3] = 0.0;
-   out_5463203357507594110[4] = 0.0;
-   out_5463203357507594110[5] = 0.0;
-   out_5463203357507594110[6] = 0.0;
-   out_5463203357507594110[7] = 0.0;
-   out_5463203357507594110[8] = 0.0;
-   out_5463203357507594110[9] = 0.0;
-   out_5463203357507594110[10] = 1.0;
-   out_5463203357507594110[11] = 0.0;
-   out_5463203357507594110[12] = 0.0;
-   out_5463203357507594110[13] = 0.0;
-   out_5463203357507594110[14] = 0.0;
-   out_5463203357507594110[15] = 0.0;
-   out_5463203357507594110[16] = 0.0;
-   out_5463203357507594110[17] = 0.0;
-   out_5463203357507594110[18] = 0.0;
-   out_5463203357507594110[19] = 0.0;
-   out_5463203357507594110[20] = 1.0;
-   out_5463203357507594110[21] = 0.0;
-   out_5463203357507594110[22] = 0.0;
-   out_5463203357507594110[23] = 0.0;
-   out_5463203357507594110[24] = 0.0;
-   out_5463203357507594110[25] = 0.0;
-   out_5463203357507594110[26] = 0.0;
-   out_5463203357507594110[27] = 0.0;
-   out_5463203357507594110[28] = 0.0;
-   out_5463203357507594110[29] = 0.0;
-   out_5463203357507594110[30] = 1.0;
-   out_5463203357507594110[31] = 0.0;
-   out_5463203357507594110[32] = 0.0;
-   out_5463203357507594110[33] = 0.0;
-   out_5463203357507594110[34] = 0.0;
-   out_5463203357507594110[35] = 0.0;
-   out_5463203357507594110[36] = 0.0;
-   out_5463203357507594110[37] = 0.0;
-   out_5463203357507594110[38] = 0.0;
-   out_5463203357507594110[39] = 0.0;
-   out_5463203357507594110[40] = 1.0;
-   out_5463203357507594110[41] = 0.0;
-   out_5463203357507594110[42] = 0.0;
-   out_5463203357507594110[43] = 0.0;
-   out_5463203357507594110[44] = 0.0;
-   out_5463203357507594110[45] = 0.0;
-   out_5463203357507594110[46] = 0.0;
-   out_5463203357507594110[47] = 0.0;
-   out_5463203357507594110[48] = 0.0;
-   out_5463203357507594110[49] = 0.0;
-   out_5463203357507594110[50] = 1.0;
-   out_5463203357507594110[51] = 0.0;
-   out_5463203357507594110[52] = 0.0;
-   out_5463203357507594110[53] = 0.0;
-   out_5463203357507594110[54] = 0.0;
-   out_5463203357507594110[55] = 0.0;
-   out_5463203357507594110[56] = 0.0;
-   out_5463203357507594110[57] = 0.0;
-   out_5463203357507594110[58] = 0.0;
-   out_5463203357507594110[59] = 0.0;
-   out_5463203357507594110[60] = 1.0;
-   out_5463203357507594110[61] = 0.0;
-   out_5463203357507594110[62] = 0.0;
-   out_5463203357507594110[63] = 0.0;
-   out_5463203357507594110[64] = 0.0;
-   out_5463203357507594110[65] = 0.0;
-   out_5463203357507594110[66] = 0.0;
-   out_5463203357507594110[67] = 0.0;
-   out_5463203357507594110[68] = 0.0;
-   out_5463203357507594110[69] = 0.0;
-   out_5463203357507594110[70] = 1.0;
-   out_5463203357507594110[71] = 0.0;
-   out_5463203357507594110[72] = 0.0;
-   out_5463203357507594110[73] = 0.0;
-   out_5463203357507594110[74] = 0.0;
-   out_5463203357507594110[75] = 0.0;
-   out_5463203357507594110[76] = 0.0;
-   out_5463203357507594110[77] = 0.0;
-   out_5463203357507594110[78] = 0.0;
-   out_5463203357507594110[79] = 0.0;
-   out_5463203357507594110[80] = 1.0;
+void H_mod_fun(double *state, double *out_6424885359819808556) {
+   out_6424885359819808556[0] = 1.0;
+   out_6424885359819808556[1] = 0.0;
+   out_6424885359819808556[2] = 0.0;
+   out_6424885359819808556[3] = 0.0;
+   out_6424885359819808556[4] = 0.0;
+   out_6424885359819808556[5] = 0.0;
+   out_6424885359819808556[6] = 0.0;
+   out_6424885359819808556[7] = 0.0;
+   out_6424885359819808556[8] = 0.0;
+   out_6424885359819808556[9] = 0.0;
+   out_6424885359819808556[10] = 1.0;
+   out_6424885359819808556[11] = 0.0;
+   out_6424885359819808556[12] = 0.0;
+   out_6424885359819808556[13] = 0.0;
+   out_6424885359819808556[14] = 0.0;
+   out_6424885359819808556[15] = 0.0;
+   out_6424885359819808556[16] = 0.0;
+   out_6424885359819808556[17] = 0.0;
+   out_6424885359819808556[18] = 0.0;
+   out_6424885359819808556[19] = 0.0;
+   out_6424885359819808556[20] = 1.0;
+   out_6424885359819808556[21] = 0.0;
+   out_6424885359819808556[22] = 0.0;
+   out_6424885359819808556[23] = 0.0;
+   out_6424885359819808556[24] = 0.0;
+   out_6424885359819808556[25] = 0.0;
+   out_6424885359819808556[26] = 0.0;
+   out_6424885359819808556[27] = 0.0;
+   out_6424885359819808556[28] = 0.0;
+   out_6424885359819808556[29] = 0.0;
+   out_6424885359819808556[30] = 1.0;
+   out_6424885359819808556[31] = 0.0;
+   out_6424885359819808556[32] = 0.0;
+   out_6424885359819808556[33] = 0.0;
+   out_6424885359819808556[34] = 0.0;
+   out_6424885359819808556[35] = 0.0;
+   out_6424885359819808556[36] = 0.0;
+   out_6424885359819808556[37] = 0.0;
+   out_6424885359819808556[38] = 0.0;
+   out_6424885359819808556[39] = 0.0;
+   out_6424885359819808556[40] = 1.0;
+   out_6424885359819808556[41] = 0.0;
+   out_6424885359819808556[42] = 0.0;
+   out_6424885359819808556[43] = 0.0;
+   out_6424885359819808556[44] = 0.0;
+   out_6424885359819808556[45] = 0.0;
+   out_6424885359819808556[46] = 0.0;
+   out_6424885359819808556[47] = 0.0;
+   out_6424885359819808556[48] = 0.0;
+   out_6424885359819808556[49] = 0.0;
+   out_6424885359819808556[50] = 1.0;
+   out_6424885359819808556[51] = 0.0;
+   out_6424885359819808556[52] = 0.0;
+   out_6424885359819808556[53] = 0.0;
+   out_6424885359819808556[54] = 0.0;
+   out_6424885359819808556[55] = 0.0;
+   out_6424885359819808556[56] = 0.0;
+   out_6424885359819808556[57] = 0.0;
+   out_6424885359819808556[58] = 0.0;
+   out_6424885359819808556[59] = 0.0;
+   out_6424885359819808556[60] = 1.0;
+   out_6424885359819808556[61] = 0.0;
+   out_6424885359819808556[62] = 0.0;
+   out_6424885359819808556[63] = 0.0;
+   out_6424885359819808556[64] = 0.0;
+   out_6424885359819808556[65] = 0.0;
+   out_6424885359819808556[66] = 0.0;
+   out_6424885359819808556[67] = 0.0;
+   out_6424885359819808556[68] = 0.0;
+   out_6424885359819808556[69] = 0.0;
+   out_6424885359819808556[70] = 1.0;
+   out_6424885359819808556[71] = 0.0;
+   out_6424885359819808556[72] = 0.0;
+   out_6424885359819808556[73] = 0.0;
+   out_6424885359819808556[74] = 0.0;
+   out_6424885359819808556[75] = 0.0;
+   out_6424885359819808556[76] = 0.0;
+   out_6424885359819808556[77] = 0.0;
+   out_6424885359819808556[78] = 0.0;
+   out_6424885359819808556[79] = 0.0;
+   out_6424885359819808556[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_6125045598488451197) {
-   out_6125045598488451197[0] = state[0];
-   out_6125045598488451197[1] = state[1];
-   out_6125045598488451197[2] = state[2];
-   out_6125045598488451197[3] = state[3];
-   out_6125045598488451197[4] = state[4];
-   out_6125045598488451197[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_6125045598488451197[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_6125045598488451197[7] = state[7];
-   out_6125045598488451197[8] = state[8];
+void f_fun(double *state, double dt, double *out_3758174016504973643) {
+   out_3758174016504973643[0] = state[0];
+   out_3758174016504973643[1] = state[1];
+   out_3758174016504973643[2] = state[2];
+   out_3758174016504973643[3] = state[3];
+   out_3758174016504973643[4] = state[4];
+   out_3758174016504973643[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_3758174016504973643[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_3758174016504973643[7] = state[7];
+   out_3758174016504973643[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_548031277288491442) {
-   out_548031277288491442[0] = 1;
-   out_548031277288491442[1] = 0;
-   out_548031277288491442[2] = 0;
-   out_548031277288491442[3] = 0;
-   out_548031277288491442[4] = 0;
-   out_548031277288491442[5] = 0;
-   out_548031277288491442[6] = 0;
-   out_548031277288491442[7] = 0;
-   out_548031277288491442[8] = 0;
-   out_548031277288491442[9] = 0;
-   out_548031277288491442[10] = 1;
-   out_548031277288491442[11] = 0;
-   out_548031277288491442[12] = 0;
-   out_548031277288491442[13] = 0;
-   out_548031277288491442[14] = 0;
-   out_548031277288491442[15] = 0;
-   out_548031277288491442[16] = 0;
-   out_548031277288491442[17] = 0;
-   out_548031277288491442[18] = 0;
-   out_548031277288491442[19] = 0;
-   out_548031277288491442[20] = 1;
-   out_548031277288491442[21] = 0;
-   out_548031277288491442[22] = 0;
-   out_548031277288491442[23] = 0;
-   out_548031277288491442[24] = 0;
-   out_548031277288491442[25] = 0;
-   out_548031277288491442[26] = 0;
-   out_548031277288491442[27] = 0;
-   out_548031277288491442[28] = 0;
-   out_548031277288491442[29] = 0;
-   out_548031277288491442[30] = 1;
-   out_548031277288491442[31] = 0;
-   out_548031277288491442[32] = 0;
-   out_548031277288491442[33] = 0;
-   out_548031277288491442[34] = 0;
-   out_548031277288491442[35] = 0;
-   out_548031277288491442[36] = 0;
-   out_548031277288491442[37] = 0;
-   out_548031277288491442[38] = 0;
-   out_548031277288491442[39] = 0;
-   out_548031277288491442[40] = 1;
-   out_548031277288491442[41] = 0;
-   out_548031277288491442[42] = 0;
-   out_548031277288491442[43] = 0;
-   out_548031277288491442[44] = 0;
-   out_548031277288491442[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_548031277288491442[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_548031277288491442[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_548031277288491442[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_548031277288491442[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_548031277288491442[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_548031277288491442[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_548031277288491442[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_548031277288491442[53] = -9.8100000000000005*dt;
-   out_548031277288491442[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_548031277288491442[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_548031277288491442[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_548031277288491442[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_548031277288491442[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_548031277288491442[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_548031277288491442[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_548031277288491442[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_548031277288491442[62] = 0;
-   out_548031277288491442[63] = 0;
-   out_548031277288491442[64] = 0;
-   out_548031277288491442[65] = 0;
-   out_548031277288491442[66] = 0;
-   out_548031277288491442[67] = 0;
-   out_548031277288491442[68] = 0;
-   out_548031277288491442[69] = 0;
-   out_548031277288491442[70] = 1;
-   out_548031277288491442[71] = 0;
-   out_548031277288491442[72] = 0;
-   out_548031277288491442[73] = 0;
-   out_548031277288491442[74] = 0;
-   out_548031277288491442[75] = 0;
-   out_548031277288491442[76] = 0;
-   out_548031277288491442[77] = 0;
-   out_548031277288491442[78] = 0;
-   out_548031277288491442[79] = 0;
-   out_548031277288491442[80] = 1;
+void F_fun(double *state, double dt, double *out_6192627187967339842) {
+   out_6192627187967339842[0] = 1;
+   out_6192627187967339842[1] = 0;
+   out_6192627187967339842[2] = 0;
+   out_6192627187967339842[3] = 0;
+   out_6192627187967339842[4] = 0;
+   out_6192627187967339842[5] = 0;
+   out_6192627187967339842[6] = 0;
+   out_6192627187967339842[7] = 0;
+   out_6192627187967339842[8] = 0;
+   out_6192627187967339842[9] = 0;
+   out_6192627187967339842[10] = 1;
+   out_6192627187967339842[11] = 0;
+   out_6192627187967339842[12] = 0;
+   out_6192627187967339842[13] = 0;
+   out_6192627187967339842[14] = 0;
+   out_6192627187967339842[15] = 0;
+   out_6192627187967339842[16] = 0;
+   out_6192627187967339842[17] = 0;
+   out_6192627187967339842[18] = 0;
+   out_6192627187967339842[19] = 0;
+   out_6192627187967339842[20] = 1;
+   out_6192627187967339842[21] = 0;
+   out_6192627187967339842[22] = 0;
+   out_6192627187967339842[23] = 0;
+   out_6192627187967339842[24] = 0;
+   out_6192627187967339842[25] = 0;
+   out_6192627187967339842[26] = 0;
+   out_6192627187967339842[27] = 0;
+   out_6192627187967339842[28] = 0;
+   out_6192627187967339842[29] = 0;
+   out_6192627187967339842[30] = 1;
+   out_6192627187967339842[31] = 0;
+   out_6192627187967339842[32] = 0;
+   out_6192627187967339842[33] = 0;
+   out_6192627187967339842[34] = 0;
+   out_6192627187967339842[35] = 0;
+   out_6192627187967339842[36] = 0;
+   out_6192627187967339842[37] = 0;
+   out_6192627187967339842[38] = 0;
+   out_6192627187967339842[39] = 0;
+   out_6192627187967339842[40] = 1;
+   out_6192627187967339842[41] = 0;
+   out_6192627187967339842[42] = 0;
+   out_6192627187967339842[43] = 0;
+   out_6192627187967339842[44] = 0;
+   out_6192627187967339842[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_6192627187967339842[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_6192627187967339842[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_6192627187967339842[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_6192627187967339842[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_6192627187967339842[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_6192627187967339842[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_6192627187967339842[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_6192627187967339842[53] = -9.8100000000000005*dt;
+   out_6192627187967339842[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_6192627187967339842[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_6192627187967339842[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_6192627187967339842[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_6192627187967339842[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_6192627187967339842[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_6192627187967339842[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_6192627187967339842[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_6192627187967339842[62] = 0;
+   out_6192627187967339842[63] = 0;
+   out_6192627187967339842[64] = 0;
+   out_6192627187967339842[65] = 0;
+   out_6192627187967339842[66] = 0;
+   out_6192627187967339842[67] = 0;
+   out_6192627187967339842[68] = 0;
+   out_6192627187967339842[69] = 0;
+   out_6192627187967339842[70] = 1;
+   out_6192627187967339842[71] = 0;
+   out_6192627187967339842[72] = 0;
+   out_6192627187967339842[73] = 0;
+   out_6192627187967339842[74] = 0;
+   out_6192627187967339842[75] = 0;
+   out_6192627187967339842[76] = 0;
+   out_6192627187967339842[77] = 0;
+   out_6192627187967339842[78] = 0;
+   out_6192627187967339842[79] = 0;
+   out_6192627187967339842[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_7825793012049214991) {
-   out_7825793012049214991[0] = state[6];
+void h_25(double *state, double *unused, double *out_1759823454732362499) {
+   out_1759823454732362499[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_8986251996152959258) {
-   out_8986251996152959258[0] = 0;
-   out_8986251996152959258[1] = 0;
-   out_8986251996152959258[2] = 0;
-   out_8986251996152959258[3] = 0;
-   out_8986251996152959258[4] = 0;
-   out_8986251996152959258[5] = 0;
-   out_8986251996152959258[6] = 1;
-   out_8986251996152959258[7] = 0;
-   out_8986251996152959258[8] = 0;
+void H_25(double *state, double *unused, double *out_2901904709830316879) {
+   out_2901904709830316879[0] = 0;
+   out_2901904709830316879[1] = 0;
+   out_2901904709830316879[2] = 0;
+   out_2901904709830316879[3] = 0;
+   out_2901904709830316879[4] = 0;
+   out_2901904709830316879[5] = 0;
+   out_2901904709830316879[6] = 1;
+   out_2901904709830316879[7] = 0;
+   out_2901904709830316879[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_3863850671382452828) {
-   out_3863850671382452828[0] = state[4];
-   out_3863850671382452828[1] = state[5];
+void h_24(double *state, double *unused, double *out_1305817215653664496) {
+   out_1305817215653664496[0] = state[4];
+   out_1305817215653664496[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_5428326397350636122) {
-   out_5428326397350636122[0] = 0;
-   out_5428326397350636122[1] = 0;
-   out_5428326397350636122[2] = 0;
-   out_5428326397350636122[3] = 0;
-   out_5428326397350636122[4] = 1;
-   out_5428326397350636122[5] = 0;
-   out_5428326397350636122[6] = 0;
-   out_5428326397350636122[7] = 0;
-   out_5428326397350636122[8] = 0;
-   out_5428326397350636122[9] = 0;
-   out_5428326397350636122[10] = 0;
-   out_5428326397350636122[11] = 0;
-   out_5428326397350636122[12] = 0;
-   out_5428326397350636122[13] = 0;
-   out_5428326397350636122[14] = 1;
-   out_5428326397350636122[15] = 0;
-   out_5428326397350636122[16] = 0;
-   out_5428326397350636122[17] = 0;
+void H_24(double *state, double *unused, double *out_656020888972006257) {
+   out_656020888972006257[0] = 0;
+   out_656020888972006257[1] = 0;
+   out_656020888972006257[2] = 0;
+   out_656020888972006257[3] = 0;
+   out_656020888972006257[4] = 1;
+   out_656020888972006257[5] = 0;
+   out_656020888972006257[6] = 0;
+   out_656020888972006257[7] = 0;
+   out_656020888972006257[8] = 0;
+   out_656020888972006257[9] = 0;
+   out_656020888972006257[10] = 0;
+   out_656020888972006257[11] = 0;
+   out_656020888972006257[12] = 0;
+   out_656020888972006257[13] = 0;
+   out_656020888972006257[14] = 1;
+   out_656020888972006257[15] = 0;
+   out_656020888972006257[16] = 0;
+   out_656020888972006257[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_3653635896979304131) {
-   out_3653635896979304131[0] = state[4];
+void h_30(double *state, double *unused, double *out_9220382218981250824) {
+   out_9220382218981250824[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_6467919037645710631) {
-   out_6467919037645710631[0] = 0;
-   out_6467919037645710631[1] = 0;
-   out_6467919037645710631[2] = 0;
-   out_6467919037645710631[3] = 0;
-   out_6467919037645710631[4] = 1;
-   out_6467919037645710631[5] = 0;
-   out_6467919037645710631[6] = 0;
-   out_6467919037645710631[7] = 0;
-   out_6467919037645710631[8] = 0;
+void H_30(double *state, double *unused, double *out_3031243656973556949) {
+   out_3031243656973556949[0] = 0;
+   out_3031243656973556949[1] = 0;
+   out_3031243656973556949[2] = 0;
+   out_3031243656973556949[3] = 0;
+   out_3031243656973556949[4] = 1;
+   out_3031243656973556949[5] = 0;
+   out_3031243656973556949[6] = 0;
+   out_3031243656973556949[7] = 0;
+   out_3031243656973556949[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_8655309164640149776) {
-   out_8655309164640149776[0] = state[7];
+void h_26(double *state, double *unused, double *out_2821340701567401678) {
+   out_2821340701567401678[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_5718988758682536134) {
-   out_5718988758682536134[0] = 0;
-   out_5718988758682536134[1] = 0;
-   out_5718988758682536134[2] = 0;
-   out_5718988758682536134[3] = 0;
-   out_5718988758682536134[4] = 0;
-   out_5718988758682536134[5] = 0;
-   out_5718988758682536134[6] = 0;
-   out_5718988758682536134[7] = 1;
-   out_5718988758682536134[8] = 0;
+void H_26(double *state, double *unused, double *out_6643408028704373103) {
+   out_6643408028704373103[0] = 0;
+   out_6643408028704373103[1] = 0;
+   out_6643408028704373103[2] = 0;
+   out_6643408028704373103[3] = 0;
+   out_6643408028704373103[4] = 0;
+   out_6643408028704373103[5] = 0;
+   out_6643408028704373103[6] = 0;
+   out_6643408028704373103[7] = 1;
+   out_6643408028704373103[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_5968325673074535865) {
-   out_5968325673074535865[0] = state[3];
+void h_27(double *state, double *unused, double *out_2703333307634024934) {
+   out_2703333307634024934[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_4244324966461767414) {
-   out_4244324966461767414[0] = 0;
-   out_4244324966461767414[1] = 0;
-   out_4244324966461767414[2] = 0;
-   out_4244324966461767414[3] = 1;
-   out_4244324966461767414[4] = 0;
-   out_4244324966461767414[5] = 0;
-   out_4244324966461767414[6] = 0;
-   out_4244324966461767414[7] = 0;
-   out_4244324966461767414[8] = 0;
+void H_27(double *state, double *unused, double *out_5206006968773981860) {
+   out_5206006968773981860[0] = 0;
+   out_5206006968773981860[1] = 0;
+   out_5206006968773981860[2] = 0;
+   out_5206006968773981860[3] = 1;
+   out_5206006968773981860[4] = 0;
+   out_5206006968773981860[5] = 0;
+   out_5206006968773981860[6] = 0;
+   out_5206006968773981860[7] = 0;
+   out_5206006968773981860[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_2671551803644006019) {
-   out_2671551803644006019[0] = state[1];
+void h_29(double *state, double *unused, double *out_7103533289356984798) {
+   out_7103533289356984798[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_5957687693331318447) {
-   out_5957687693331318447[0] = 0;
-   out_5957687693331318447[1] = 1;
-   out_5957687693331318447[2] = 0;
-   out_5957687693331318447[3] = 0;
-   out_5957687693331318447[4] = 0;
-   out_5957687693331318447[5] = 0;
-   out_5957687693331318447[6] = 0;
-   out_5957687693331318447[7] = 0;
-   out_5957687693331318447[8] = 0;
+void H_29(double *state, double *unused, double *out_6919369695643532893) {
+   out_6919369695643532893[0] = 0;
+   out_6919369695643532893[1] = 1;
+   out_6919369695643532893[2] = 0;
+   out_6919369695643532893[3] = 0;
+   out_6919369695643532893[4] = 0;
+   out_6919369695643532893[5] = 0;
+   out_6919369695643532893[6] = 0;
+   out_6919369695643532893[7] = 0;
+   out_6919369695643532893[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_3845497107703219871) {
-   out_3845497107703219871[0] = state[0];
+void h_28(double *state, double *unused, double *out_5740119359078357619) {
+   out_5740119359078357619[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_7406657363308702595) {
-   out_7406657363308702595[0] = 1;
-   out_7406657363308702595[1] = 0;
-   out_7406657363308702595[2] = 0;
-   out_7406657363308702595[3] = 0;
-   out_7406657363308702595[4] = 0;
-   out_7406657363308702595[5] = 0;
-   out_7406657363308702595[6] = 0;
-   out_7406657363308702595[7] = 0;
-   out_7406657363308702595[8] = 0;
+void H_28(double *state, double *unused, double *out_4955739424078206642) {
+   out_4955739424078206642[0] = 1;
+   out_4955739424078206642[1] = 0;
+   out_4955739424078206642[2] = 0;
+   out_4955739424078206642[3] = 0;
+   out_4955739424078206642[4] = 0;
+   out_4955739424078206642[5] = 0;
+   out_4955739424078206642[6] = 0;
+   out_4955739424078206642[7] = 0;
+   out_4955739424078206642[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_1981073584941794741) {
-   out_1981073584941794741[0] = state[8];
+void h_31(double *state, double *unused, double *out_1602636786381233354) {
+   out_1602636786381233354[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_8955606034275998830) {
-   out_8955606034275998830[0] = 0;
-   out_8955606034275998830[1] = 0;
-   out_8955606034275998830[2] = 0;
-   out_8955606034275998830[3] = 0;
-   out_8955606034275998830[4] = 0;
-   out_8955606034275998830[5] = 0;
-   out_8955606034275998830[6] = 0;
-   out_8955606034275998830[7] = 0;
-   out_8955606034275998830[8] = 1;
+void H_31(double *state, double *unused, double *out_2871258747953356451) {
+   out_2871258747953356451[0] = 0;
+   out_2871258747953356451[1] = 0;
+   out_2871258747953356451[2] = 0;
+   out_2871258747953356451[3] = 0;
+   out_2871258747953356451[4] = 0;
+   out_2871258747953356451[5] = 0;
+   out_2871258747953356451[6] = 0;
+   out_2871258747953356451[7] = 0;
+   out_2871258747953356451[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_7353202191454035649) {
-  err_fun(nom_x, delta_x, out_7353202191454035649);
+void car_err_fun(double *nom_x, double *delta_x, double *out_2869805078350318634) {
+  err_fun(nom_x, delta_x, out_2869805078350318634);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_5273149315250963980) {
-  inv_err_fun(nom_x, true_x, out_5273149315250963980);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_6624138795093603288) {
+  inv_err_fun(nom_x, true_x, out_6624138795093603288);
 }
-void car_H_mod_fun(double *state, double *out_5463203357507594110) {
-  H_mod_fun(state, out_5463203357507594110);
+void car_H_mod_fun(double *state, double *out_6424885359819808556) {
+  H_mod_fun(state, out_6424885359819808556);
 }
-void car_f_fun(double *state, double dt, double *out_6125045598488451197) {
-  f_fun(state,  dt, out_6125045598488451197);
+void car_f_fun(double *state, double dt, double *out_3758174016504973643) {
+  f_fun(state,  dt, out_3758174016504973643);
 }
-void car_F_fun(double *state, double dt, double *out_548031277288491442) {
-  F_fun(state,  dt, out_548031277288491442);
+void car_F_fun(double *state, double dt, double *out_6192627187967339842) {
+  F_fun(state,  dt, out_6192627187967339842);
 }
-void car_h_25(double *state, double *unused, double *out_7825793012049214991) {
-  h_25(state, unused, out_7825793012049214991);
+void car_h_25(double *state, double *unused, double *out_1759823454732362499) {
+  h_25(state, unused, out_1759823454732362499);
 }
-void car_H_25(double *state, double *unused, double *out_8986251996152959258) {
-  H_25(state, unused, out_8986251996152959258);
+void car_H_25(double *state, double *unused, double *out_2901904709830316879) {
+  H_25(state, unused, out_2901904709830316879);
 }
-void car_h_24(double *state, double *unused, double *out_3863850671382452828) {
-  h_24(state, unused, out_3863850671382452828);
+void car_h_24(double *state, double *unused, double *out_1305817215653664496) {
+  h_24(state, unused, out_1305817215653664496);
 }
-void car_H_24(double *state, double *unused, double *out_5428326397350636122) {
-  H_24(state, unused, out_5428326397350636122);
+void car_H_24(double *state, double *unused, double *out_656020888972006257) {
+  H_24(state, unused, out_656020888972006257);
 }
-void car_h_30(double *state, double *unused, double *out_3653635896979304131) {
-  h_30(state, unused, out_3653635896979304131);
+void car_h_30(double *state, double *unused, double *out_9220382218981250824) {
+  h_30(state, unused, out_9220382218981250824);
 }
-void car_H_30(double *state, double *unused, double *out_6467919037645710631) {
-  H_30(state, unused, out_6467919037645710631);
+void car_H_30(double *state, double *unused, double *out_3031243656973556949) {
+  H_30(state, unused, out_3031243656973556949);
 }
-void car_h_26(double *state, double *unused, double *out_8655309164640149776) {
-  h_26(state, unused, out_8655309164640149776);
+void car_h_26(double *state, double *unused, double *out_2821340701567401678) {
+  h_26(state, unused, out_2821340701567401678);
 }
-void car_H_26(double *state, double *unused, double *out_5718988758682536134) {
-  H_26(state, unused, out_5718988758682536134);
+void car_H_26(double *state, double *unused, double *out_6643408028704373103) {
+  H_26(state, unused, out_6643408028704373103);
 }
-void car_h_27(double *state, double *unused, double *out_5968325673074535865) {
-  h_27(state, unused, out_5968325673074535865);
+void car_h_27(double *state, double *unused, double *out_2703333307634024934) {
+  h_27(state, unused, out_2703333307634024934);
 }
-void car_H_27(double *state, double *unused, double *out_4244324966461767414) {
-  H_27(state, unused, out_4244324966461767414);
+void car_H_27(double *state, double *unused, double *out_5206006968773981860) {
+  H_27(state, unused, out_5206006968773981860);
 }
-void car_h_29(double *state, double *unused, double *out_2671551803644006019) {
-  h_29(state, unused, out_2671551803644006019);
+void car_h_29(double *state, double *unused, double *out_7103533289356984798) {
+  h_29(state, unused, out_7103533289356984798);
 }
-void car_H_29(double *state, double *unused, double *out_5957687693331318447) {
-  H_29(state, unused, out_5957687693331318447);
+void car_H_29(double *state, double *unused, double *out_6919369695643532893) {
+  H_29(state, unused, out_6919369695643532893);
 }
-void car_h_28(double *state, double *unused, double *out_3845497107703219871) {
-  h_28(state, unused, out_3845497107703219871);
+void car_h_28(double *state, double *unused, double *out_5740119359078357619) {
+  h_28(state, unused, out_5740119359078357619);
 }
-void car_H_28(double *state, double *unused, double *out_7406657363308702595) {
-  H_28(state, unused, out_7406657363308702595);
+void car_H_28(double *state, double *unused, double *out_4955739424078206642) {
+  H_28(state, unused, out_4955739424078206642);
 }
-void car_h_31(double *state, double *unused, double *out_1981073584941794741) {
-  h_31(state, unused, out_1981073584941794741);
+void car_h_31(double *state, double *unused, double *out_1602636786381233354) {
+  h_31(state, unused, out_1602636786381233354);
 }
-void car_H_31(double *state, double *unused, double *out_8955606034275998830) {
-  H_31(state, unused, out_8955606034275998830);
+void car_H_31(double *state, double *unused, double *out_2871258747953356451) {
+  H_31(state, unused, out_2871258747953356451);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
   predict(in_x, in_P, in_Q, dt);

openpilot/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_7353202191454035649);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_5273149315250963980);
-void car_H_mod_fun(double *state, double *out_5463203357507594110);
-void car_f_fun(double *state, double dt, double *out_6125045598488451197);
-void car_F_fun(double *state, double dt, double *out_548031277288491442);
-void car_h_25(double *state, double *unused, double *out_7825793012049214991);
-void car_H_25(double *state, double *unused, double *out_8986251996152959258);
-void car_h_24(double *state, double *unused, double *out_3863850671382452828);
-void car_H_24(double *state, double *unused, double *out_5428326397350636122);
-void car_h_30(double *state, double *unused, double *out_3653635896979304131);
-void car_H_30(double *state, double *unused, double *out_6467919037645710631);
-void car_h_26(double *state, double *unused, double *out_8655309164640149776);
-void car_H_26(double *state, double *unused, double *out_5718988758682536134);
-void car_h_27(double *state, double *unused, double *out_5968325673074535865);
-void car_H_27(double *state, double *unused, double *out_4244324966461767414);
-void car_h_29(double *state, double *unused, double *out_2671551803644006019);
-void car_H_29(double *state, double *unused, double *out_5957687693331318447);
-void car_h_28(double *state, double *unused, double *out_3845497107703219871);
-void car_H_28(double *state, double *unused, double *out_7406657363308702595);
-void car_h_31(double *state, double *unused, double *out_1981073584941794741);
-void car_H_31(double *state, double *unused, double *out_8955606034275998830);
+void car_err_fun(double *nom_x, double *delta_x, double *out_2869805078350318634);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_6624138795093603288);
+void car_H_mod_fun(double *state, double *out_6424885359819808556);
+void car_f_fun(double *state, double dt, double *out_3758174016504973643);
+void car_F_fun(double *state, double dt, double *out_6192627187967339842);
+void car_h_25(double *state, double *unused, double *out_1759823454732362499);
+void car_H_25(double *state, double *unused, double *out_2901904709830316879);
+void car_h_24(double *state, double *unused, double *out_1305817215653664496);
+void car_H_24(double *state, double *unused, double *out_656020888972006257);
+void car_h_30(double *state, double *unused, double *out_9220382218981250824);
+void car_H_30(double *state, double *unused, double *out_3031243656973556949);
+void car_h_26(double *state, double *unused, double *out_2821340701567401678);
+void car_H_26(double *state, double *unused, double *out_6643408028704373103);
+void car_h_27(double *state, double *unused, double *out_2703333307634024934);
+void car_H_27(double *state, double *unused, double *out_5206006968773981860);
+void car_h_29(double *state, double *unused, double *out_7103533289356984798);
+void car_H_29(double *state, double *unused, double *out_6919369695643532893);
+void car_h_28(double *state, double *unused, double *out_5740119359078357619);
+void car_H_28(double *state, double *unused, double *out_4955739424078206642);
+void car_h_31(double *state, double *unused, double *out_1602636786381233354);
+void car_H_31(double *state, double *unused, double *out_2871258747953356451);
 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);

openpilot/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_2134324718959906218);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7874553077816062449);
-void pose_H_mod_fun(double *state, double *out_8129875391940694982);
-void pose_f_fun(double *state, double dt, double *out_5920058179701921818);
-void pose_F_fun(double *state, double dt, double *out_534012189001621552);
-void pose_h_4(double *state, double *unused, double *out_6191552396506608484);
-void pose_H_4(double *state, double *unused, double *out_4468786875016897913);
-void pose_h_10(double *state, double *unused, double *out_8347226084972414573);
-void pose_H_10(double *state, double *unused, double *out_4111169058602364742);
-void pose_h_13(double *state, double *unused, double *out_2707578914264125574);
-void pose_H_13(double *state, double *unused, double *out_7681060700349230714);
-void pose_h_14(double *state, double *unused, double *out_4697260034120461682);
-void pose_H_14(double *state, double *unused, double *out_8432027731356382442);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_4327736854274805720);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_542433210393079092);
+void pose_H_mod_fun(double *state, double *out_2780673785235724857);
+void pose_f_fun(double *state, double dt, double *out_2987768017781358156);
+void pose_F_fun(double *state, double dt, double *out_9215256453153547448);
+void pose_h_4(double *state, double *unused, double *out_3412232203917073472);
+void pose_H_4(double *state, double *unused, double *out_3534834787290437464);
+void pose_h_10(double *state, double *unused, double *out_115890013071339636);
+void pose_H_10(double *state, double *unused, double *out_6130958711041943396);
+void pose_h_13(double *state, double *unused, double *out_6371690278926638796);
+void pose_H_13(double *state, double *unused, double *out_7301278078102413223);
+void pose_h_14(double *state, double *unused, double *out_6221009380571624691);
+void pose_H_14(double *state, double *unused, double *out_7498075643629921993);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

openpilot/selfdrive/locationd/test/test_torqued.py

@@ -1,6 +1,26 @@
+import numpy as np
+
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.hyundai.values import CAR as HYUNDAI
 from opendbc.car.structs import car
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.selfdrive.locationd.torqued import TorqueEstimator
+from openpilot.selfdrive.locationd.torqued import (TorqueEstimator, LAT_ACC_THRESHOLD, FACTOR_SANITY,
+                                                   IONIQ_6_LAT_ACC_THRESHOLD, IONIQ_6_FACTOR_SANITY)
+
+
+def _fill_line(est, slope, seed=0):
+  rng = np.random.default_rng(seed)
+  for (low, high), min_pts in zip(est.filtered_points.buckets.keys(),
+                                  est.filtered_points.buckets_min_points.values(), strict=True):
+    for _ in range(int(min_pts)):
+      x = rng.uniform(low, high)
+      est.filtered_points.add_point(x, slope * x + rng.normal(0.0, 0.02))
+  keys = list(est.filtered_points.buckets)
+  i = 0
+  while len(est.filtered_points) < est.min_points_total:
+    x = rng.uniform(*keys[i % len(keys)])
+    est.filtered_points.add_point(x, slope * x + rng.normal(0.0, 0.02))
+    i += 1
 
 
 class TestTorqued(OpenpilotTestCase):
@@ -25,3 +45,25 @@ class TestTorqued(OpenpilotTestCase):
 
     msg = est.get_msg()
     assert msg.lateralTorqueParameters.calPerc == 100
+
+  def test_ioniq_6_learner_limits(self):
+    cp = interfaces[HYUNDAI.HYUNDAI_IONIQ_6].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_6)
+    est = TorqueEstimator(cp)
+    factor = cp.lateralTuning.torque.latAccelFactor
+    assert est.lat_acc_threshold == IONIQ_6_LAT_ACC_THRESHOLD
+    assert np.isclose(est.max_lataccel_factor, factor * (1 + IONIQ_6_FACTOR_SANITY))
+
+    other = TorqueEstimator(interfaces[HYUNDAI.HYUNDAI_IONIQ_5].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_5))
+    assert other.lat_acc_threshold == LAT_ACC_THRESHOLD
+    assert other.factor_sanity == FACTOR_SANITY
+
+  def test_ioniq_6_learned_slope_is_not_clamped(self):
+    cp = interfaces[HYUNDAI.HYUNDAI_IONIQ_6].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_6)
+    est = TorqueEstimator(cp)
+    slope = cp.lateralTuning.torque.latAccelFactor * 1.45
+    _fill_line(est, slope)
+    captured = {}
+    est.update_params = lambda params: captured.update(params)
+    msg = est.get_msg()
+    assert msg.lateralTorqueParameters.valid
+    assert abs(captured["latAccelFactor"] - slope) < 0.1

openpilot/selfdrive/locationd/torqued.py

@@ -32,6 +32,9 @@ STEER_MIN_THRESHOLD = 0.02
 MIN_FILTER_DECAY = 50
 MAX_FILTER_DECAY = 250
 LAT_ACC_THRESHOLD = 1
+# Ioniq 6 makes ~4.5 m/s^2 per unit torque; the default cap empties its outer buckets
+IONIQ_6_LAT_ACC_THRESHOLD = 2.5
+IONIQ_6_FACTOR_SANITY = 0.5
 STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)]
 MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100])
 MIN_ENGAGE_BUFFER = 2  # secs
@@ -73,6 +76,11 @@ class TorqueEstimator(ParameterEstimator):
       self.factor_sanity = FACTOR_SANITY
       self.friction_sanity = FRICTION_SANITY
 
+    self.lat_acc_threshold = LAT_ACC_THRESHOLD
+    if str(CP.carFingerprint) == 'HYUNDAI_IONIQ_6':
+      self.lat_acc_threshold = IONIQ_6_LAT_ACC_THRESHOLD
+      self.factor_sanity = max(self.factor_sanity, IONIQ_6_FACTOR_SANITY)
+
     self.offline_friction = 0.0
     self.offline_latAccelFactor = 0.0
     self.resets = 0.0
@@ -211,7 +219,7 @@ class TorqueEstimator(ParameterEstimator):
         steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque']).item()
         lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY).item()
         if all(lat_active) and not any(steer_override) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD):
-          if abs(lateral_acc) <= LAT_ACC_THRESHOLD:
+          if abs(lateral_acc) <= self.lat_acc_threshold:
             self.filtered_points.add_point(steer, lateral_acc)
 
           if self.track_all_points:

openpilot/starpilot/car/hyundai/dashboard.py

@@ -0,0 +1,32 @@
+"""Optional Hyundai CAN-FD camera stop-sign and EV drive-mode observations; never a longitudinal control grant."""
+import time
+
+from opendbc.car.hyundai.values import HyundaiFlags
+from openpilot.starpilot.car.mazda.dashboard import write_stop_sign
+
+DRIVE_MODE_MAX_AGE_NS = 2_000_000_000
+DRIVE_MODE_ECO = 4
+DRIVE_MODE_SPORT = 5
+
+
+def capable(cp):
+  return cp.brand == 'hyundai' and bool(int(cp.flags) & HyundaiFlags.CANFD) and not cp.notCar
+
+
+def compose(cp, parsed, state, message, *, car_state_stamp):
+  if not capable(cp):
+    return message
+  message = write_stop_sign(parsed, state, message, car_state_stamp=car_state_stamp)
+  return write_drive_mode(parsed, state, message)
+
+
+def write_drive_mode(parsed, state, message):
+  observation = getattr(parsed, 'drive_mode', None)
+  if type(observation) is not tuple or len(observation) != 2 or not state.canValid or state.canTimeout:
+    return message
+  mode, stamp = observation
+  age = time.clock_gettime_ns(getattr(time, 'CLOCK_BOOTTIME', time.CLOCK_MONOTONIC)) - stamp
+  if 0 <= age <= DRIVE_MODE_MAX_AGE_NS:
+    message.starpilotCarState.ecoGear = mode == DRIVE_MODE_ECO
+    message.starpilotCarState.sportGear = mode == DRIVE_MODE_SPORT
+  return message

openpilot/starpilot/car/mazda/dashboard.py

@@ -17,6 +17,10 @@ def capable(cp):
 def compose(cp, parsed, state, message, *, car_state_stamp):
   if not capable(cp):
     return message
+  return write_stop_sign(parsed, state, message, car_state_stamp=car_state_stamp)
+
+
+def write_stop_sign(parsed, state, message, *, car_state_stamp):
   if message is None:
     message = messaging.new_message('starpilotCarState', valid=bool(state.canValid), logMonoTime=car_state_stamp)
     message.starpilotCarState.sourceCarStateMonoTime = car_state_stamp

openpilot/starpilot/conditional_mode/policy.py

@@ -407,6 +407,8 @@ class ConditionalModePolicy:
 
   def _cem(self, now_s: float, authority: Authority, warm: bool, scene: SceneEvidence, settings: ModeSettings) -> Decision:
     committed = settings.cem_stop and scene.forcing_stop is True and authority.driving_enabled and authority.long_active
+    if scene.standstill is True and scene.standstill_stop_hold is None and not committed and self.cem_previous_stop_hold:
+      return self._decision(True, authority, True, warm, Reason.CEM_STOP, 8)
     if not _bool(scene.standstill) or (scene.standstill and not committed and not _bool(scene.standstill_stop_hold)):
       self._reset_cem()
       return self._decision(False, authority, True, warm, Reason.SCENE_UNAVAILABLE, 0)

openpilot/starpilot/conditional_mode/tests/test_policy.py

@@ -210,17 +210,21 @@ class ConditionalModePolicyTest(unittest.TestCase):
         stop = policy.step(12.2, ModeChoice.CEM, ManualIntent.NONE, AUTHORITY, scene(12.2, speed=10.0, stop_light_detected=True), settings)
         self.assertEqual((stop.reason, stop.status_code), (Reason.CEM_STOP, 8))
 
-  def test_cem_unknown_stop_and_scene_loss_clear_lifecycle_state(self):
+  def test_cem_unknown_standstill_scene_keeps_existing_stop_hold(self):
     policy = ConditionalModePolicy()
     settings = replace(ModeSettings(), cem_speed_mps=15.0)
+    cold = policy.step(9.95, ModeChoice.CEM, ManualIntent.NONE, AUTHORITY, scene(9.95, speed=0.0, standstill=True), settings)
+    self.assertEqual((cold.reason, cold.requested_experimental), (Reason.SCENE_UNAVAILABLE, False))
     policy.step(10.0, ModeChoice.CEM, ManualIntent.NONE, AUTHORITY, scene(10.0, speed=0.0, standstill=True, standstill_stop_hold=True), settings)
-    unknown = policy.step(10.05, ModeChoice.CEM, ManualIntent.NONE, AUTHORITY, scene(10.05, speed=0.0, standstill=True), settings)
-    self.assertEqual((unknown.reason, unknown.requested_experimental), (Reason.SCENE_UNAVAILABLE, False))
-    # Missing decisive stop evidence clears remembered latches, not an invented release.
-    resumed = policy.step(10.1, ModeChoice.CEM, ManualIntent.NONE, AUTHORITY, scene(10.1, speed=10.0), settings)
-    self.assertEqual(resumed.reason, Reason.CEM_SPEED)
-    policy.step(10.15, ModeChoice.CEM, ManualIntent.NONE, AUTHORITY, scene(9.0, speed=10.0), settings)
-    cleared = policy.step(10.2, ModeChoice.CEM, ManualIntent.NONE, AUTHORITY, scene(10.2, speed=20.0), settings)
+    for tick in range(1, 21):
+      now = 10.0 + tick * 0.05
+      unknown = policy.step(now, ModeChoice.CEM, ManualIntent.NONE, AUTHORITY, scene(now, speed=0.0, standstill=True), settings)
+      self.assertEqual((unknown.reason, unknown.requested_experimental), (Reason.CEM_STOP, True))
+    released = policy.step(11.05, ModeChoice.CEM, ManualIntent.NONE, AUTHORITY,
+                           scene(11.05, speed=0.0, standstill=True, standstill_stop_hold=False), settings)
+    self.assertFalse(released.requested_experimental)
+    policy.step(11.1, ModeChoice.CEM, ManualIntent.NONE, AUTHORITY, scene(9.0, speed=10.0), settings)
+    cleared = policy.step(11.15, ModeChoice.CEM, ManualIntent.NONE, AUTHORITY, scene(11.15, speed=20.0), settings)
     self.assertFalse(cleared.requested_experimental)
 
   def test_cem_manual_override_preserves_observed_stop_release_quiet_period(self):

openpilot/starpilot/feature_runtime.py

@@ -14,6 +14,7 @@ from openpilot.starpilot.aol.intent import independent_axis_requested, read_sett
 from openpilot.starpilot.aol.vehicle import policy_for as aol_policy_for
 from openpilot.starpilot.feature_capabilities import display_supported, longitudinal_supported
 from openpilot.starpilot.longitudinal.profile_runtime import read_settings as read_profile_settings, selected_profiles_requested, read_global_powertrain_preset
+from openpilot.starpilot.longitudinal.drive_mode import KEY as DRIVE_MODE_KEY
 from openpilot.starpilot.longitudinal.profile_preferences import read_document_value
 from openpilot.starpilot.saved_source import read_saved
 from openpilot.starpilot.speed_limits.runtime_settings import Settings as SlcSettings, read_params as read_slc_settings
@@ -52,7 +53,8 @@ def requested(params, feature: str, *, include_auxiliary: bool = False, saved_sl
     document = read_document_value(params)
     return (read_profile_settings(params) is not None or
             (document.valid and isinstance(document.value, dict) and
-             selected_profiles_requested(document.value)))
+             selected_profiles_requested(document.value)) or
+            (document.valid and _read(params, DRIVE_MODE_KEY, 8) == (b'1', True)))
   if feature in ('slc', 'vision'):
     settings = read_slc_settings(params) if saved_slc is None else saved_slc
     return (settings.enabled or settings.display) if feature == 'slc' else (

openpilot/starpilot/flm/_primitive_walk.binding.json

@@ -1 +1 @@
-{"abi":{"SOABI":"cpython-312-aarch64-linux-gnu","cache_tag":"cpython-312","implementation":"cpython","machine":"aarch64","platform":"linux","pointer_bits":64,"python":[3,12]},"artifact_sha256":"784d1e4627399be5e2b4953c8d406331f534a902dfce01904a17981980de6b5f","revision":"flm-exact-primitives-v1","schema":1,"sources":{"SConscript":"8e7589098349bc9400c9b99578fcdf661db204578762b2121ce9d2a1d34b90bd","SConstruct":"6b51ee8b7f0de882c6d40d02f2c18ac710479ad6452a6457d5d2e3d04d7d7f0f","_primitive_walk.pyx":"564a8f5d0d0d1ac3af6cc873d7502440000a17e8609a1c900d0f90b09fb86a03","native_primitives.py":"1f197b261dde6219735d2d832ef7129c6f9fe8b9e415498115d722c74649acbf"}}
+{"abi":{"SOABI":"cpython-312-aarch64-linux-gnu","cache_tag":"cpython-312","implementation":"cpython","machine":"aarch64","platform":"linux","pointer_bits":64,"python":[3,12]},"artifact_sha256":"63a570af4a8c46e86156f10f4db26f16d637b0cf5691b7b8e37829c26778447d","revision":"flm-exact-primitives-v1","schema":1,"sources":{"SConscript":"8e7589098349bc9400c9b99578fcdf661db204578762b2121ce9d2a1d34b90bd","SConstruct":"6b51ee8b7f0de882c6d40d02f2c18ac710479ad6452a6457d5d2e3d04d7d7f0f","_primitive_walk.pyx":"564a8f5d0d0d1ac3af6cc873d7502440000a17e8609a1c900d0f90b09fb86a03","native_primitives.py":"1f197b261dde6219735d2d832ef7129c6f9fe8b9e415498115d722c74649acbf"}}

openpilot/starpilot/galaxy/settings.py

@@ -41,7 +41,7 @@ from openpilot.starpilot.ui.vasm_owner import ANNOTATION as VASM_ANNOTATION, RES
 
 PAGES = frozenset(("hub", "localization", "developer", "vehicle", "slc", "lane", "lane_change", "profiles", "personalities",
                    "traffic", "aggressive", "standard", "relaxed",
-                   "curve", "torque", "torque/bluepilot", "aol", "wheel", "appearance", "display", "data", "power", "sounds", "pip", "sentry", "vasm",
+                   "curve", "torque", "torque/bluepilot", "highway_smoothing", "aol", "wheel", "appearance", "display", "data", "power", "cooling", "sounds", "pip", "sentry", "vasm",
                    "conditional", "conditional/cem", "conditional/ccm",
                    *(f"{name}/{category}" for name in ("traffic", "aggressive", "standard", "relaxed")
                      for category in ("acceleration", "braking", "following"))))
@@ -58,7 +58,7 @@ CAR_CONTROL_SECTIONS = (
   ("wheel", "Wheel Controls", ("wheel",)),
   ("visual", "Visual (Display & UI)", ("appearance", "pip")),
   ("sounds", "Sounds & Alerts", ("sounds",)),
-  ("device", "Device & Data", ("display", "data")),
+  ("device", "Device & Data", ("display", "data", "cooling")),
   ("localization", "Localization", ("localization",)),
   ("developer", "Developer", ("developer",)),
 )
@@ -499,7 +499,7 @@ class SettingsGateway:
     with self.lock:
       ctx = self.context.sample()
       sections = []
-      for page in ("torque", "lane", "lane_change", "aol", "conditional", "profiles", "slc", "curve"):
+      for page in ("torque", "highway_smoothing", "lane", "lane_change", "aol", "conditional", "profiles", "slc", "curve"):
         state = self._state(page, ctx)
         sections.append({"title": state.title,
                          "rows": [{"label": row.label, "value": row.value} for row in state.rows[:64]]})
@@ -684,6 +684,7 @@ class SettingsGateway:
     scope = hashlib.sha256(_session_key(token) + generation + (ctx.cp_raw or b"")).digest()
     result = {"page": page, "title": state.title, "subtitle": state.subtitle, "parked": state.parked,
               "vehicleIdentified": bool(ctx.cp is not None and getattr(ctx.cp, "carFingerprint", None)),
+              "coolingAvailable": HARDWARE.get_device_type() in ("tici", "tizi"),
               "rows": [{**_projection(row), "revision": hashlib.sha256(scope + repr(row).encode()).hexdigest()}
                        for row in state.rows], "view": view_id}
     if page == "hub" or any(row.page for row in state.rows):

openpilot/starpilot/galaxy/tests/test_settings_controls.mjs

@@ -194,7 +194,7 @@ for (const id of ["wheel", "sounds", "localization"]) {
 }
 
 const deviceData = SETTINGS_SECTIONS.find((item) => item.id === "device")
-assert.deepEqual(deviceData.pages, ["power", "display", "data"])
+assert.deepEqual(deviceData.pages, ["power", "display", "data", "cooling"])
 assert.equal(SettingsPage.computed.atSectionRoot.call({ activeSection: deviceData,
   state: { data: { page: "hub" } } }), true)
 assert.equal(SettingsPage.computed.atSectionRoot.call({ activeSection: deviceData,
@@ -203,6 +203,9 @@ assert.equal(SettingsPage.computed.atSectionRoot.call({ activeSection: deviceDat
 const dataLinks = SettingsPage.computed.visibleRows.call({ initialPage: "hub", activeSection: deviceData,
   state: { query: "", data: { page: "hub", rows: [] } } })
 assert.deepEqual(dataLinks.map(({ row }) => row.page), ["power", "display", "data"])
+const coolingLinks = SettingsPage.computed.visibleRows.call({ initialPage: "hub", activeSection: deviceData,
+  state: { query: "", data: { page: "hub", rows: [], coolingAvailable: true } } })
+assert.deepEqual(coolingLinks.map(({ row }) => row.page), ["power", "display", "data", "cooling"])
 
 assert.equal(SettingsPage.computed.atSectionRoot.call({initialPage:'hub',state:{data:null,parents:[]}}),true,
   'section tabs stay mounted while loading a new section')

openpilot/starpilot/galaxy/web/js/settings.js

@@ -17,12 +17,12 @@ const post = (fetcher, path, body, signal) => fetcher(path, {
 })
 
 export const SETTINGS_SECTIONS = Object.freeze([
-  { id: "lateral", label: "Lateral (Steering)", icon: "bi-arrows-move", pages: ["aol", "lane_change", "torque", "lane"] },
+  { id: "lateral", label: "Lateral (Steering)", icon: "bi-arrows-move", pages: ["aol", "lane_change", "torque", "highway_smoothing", "lane"] },
   { id: "longitudinal", label: "Longitudinal (Speed & Following)", icon: "bi-speedometer2", pages: ["profiles", "conditional", "curve", "slc", "personalities", "aggressive", "standard", "relaxed", "traffic"] },
   { id: "wheel", label: "Wheel Controls", icon: "bi-controller", pages: ["wheel"] },
   { id: "visual", label: "Visual (Display & UI)", icon: "bi-eye", pages: ["appearance", "ui_layout", "favorites", "pip"] },
   { id: "sounds", label: "Sounds & Alerts", icon: "bi-volume-up", pages: ["sounds"] },
-  { id: "device", label: "Device & Data", icon: "bi-hdd", pages: ["power", "display", "data"] },
+  { id: "device", label: "Device & Data", icon: "bi-hdd", pages: ["power", "display", "data", "cooling"] },
   { id: "localization", label: "Localization", icon: "bi-globe", pages: ["localization"] },
   { id: "developer", label: "Developer", icon: "bi-code-slash", pages: ["developer"] },
 ])
@@ -34,7 +34,8 @@ export const isPipCropRow = (row) => ["Visual camera crop editor", "Camera crop
   /^Vehicle (left|right) crop/.test(row.label || "")
 
 const SECTION_LINKS = {
-  device: [{ label: "Parked Power", page: "power", value: "Adjust shutdown hours and battery-voltage cutoff" }, { label: "Display", page: "display", value: "Adjust screen brightness and display timing" }, { label: "Data Uploads", page: "data", value: "Choose mobile-data uploads and usage-statistics sharing" }],
+  device: [{ label: "Parked Power", page: "power", value: "Adjust shutdown hours and battery-voltage cutoff" }, { label: "Display", page: "display", value: "Adjust screen brightness and display timing" }, { label: "Data Uploads", page: "data", value: "Choose mobile-data uploads and usage-statistics sharing" },
+    { label: "Cooling", page: "cooling", value: "Choose the fan curve for comma 3/3X", requires: "coolingAvailable" }],
   visual: [{ label: "Driving Screen Widgets, Colors & Layout", page: "ui_layout" }, { label: "Quick Select", page: "favorites" }, { label: "Blind Spot Camera and Preview", page: "pip" }],
 }
 
@@ -293,6 +294,7 @@ export const SettingsPage = {
       const section = this.activeSection
       const entries = inHub && SECTION_LINKS[section.id]
         ? SECTION_LINKS[section.id].map((row, index) => ({ row: { ...row, available: true, action: false, value: row.value || "Customize " + row.label.toLocaleLowerCase() }, index }))
+          .filter(({ row }) => !row.requires || this.state.data?.[row.requires])
         : rows.map((row, index) => ({ row, index })).filter(({ row }) => !inHub || section.pages.includes(row.page))
       if (inHub && section.id === "longitudinal") {
         entries.sort((a, b) => section.pages.indexOf(a.row.page) - section.pages.indexOf(b.row.page))

openpilot/starpilot/longitudinal/drive_mode.py

@@ -0,0 +1,41 @@
+"""Vehicle drive mode (Eco/Normal/Sport) selects the global acceleration and deceleration profiles."""
+
+KEY = 'DriveModeProfiles'
+ACCELERATION_KEY = 'DriveModeAcceleration'
+DECELERATION_KEY = 'DriveModeDeceleration'
+SOURCE_AGE_NS = 500_000_000
+PROFILES = {
+  'eco': ('eco', 'eco'),
+  'normal': ('standard', 'standard'),
+  'sport': ('sport_plus', 'sport'),
+}
+
+
+def read_enabled(params) -> tuple[bool, bool]:
+  from openpilot.starpilot.saved_source import read_saved
+  if read_saved(params, KEY, 8) != (b'1', True):
+    return False, False
+  return tuple(read_saved(params, key, 8) in ((None, True), (b'1', True)) for key in (ACCELERATION_KEY, DECELERATION_KEY))
+
+
+def observe(sm, *, now_ns: int) -> str | None:
+  try:
+    if not sm.seen['starpilotCarState'] or not sm.valid['starpilotCarState']:
+      return None
+    if not 0 <= now_ns - int(sm.logMonoTime['starpilotCarState']) <= SOURCE_AGE_NS:
+      return None
+    state = sm['starpilotCarState']
+    return 'eco' if state.ecoGear else 'sport' if state.sportGear else 'normal'
+  except (AttributeError, KeyError, TypeError, ValueError):
+    return None
+
+
+def apply(document: dict, drive_mode: str | None, *, acceleration: bool = True, deceleration: bool = True) -> dict:
+  if drive_mode not in PROFILES:
+    return document
+  selected_acceleration, selected_deceleration = PROFILES[drive_mode]
+  if acceleration:
+    document = {**document, 'selectedAccelerationProfile': selected_acceleration}
+  if deceleration:
+    document = {**document, 'selectedDecelerationProfile': selected_deceleration}
+  return document

openpilot/starpilot/longitudinal/force_stop.py

@@ -5,6 +5,14 @@ import math
 
 from openpilot.common.realtime import DT_MDL
 
+STANDSTILL_HOLD_NS = 2_000_000_000
+STANDSTILL_CLEAR_NS = 750_000_000
+SIGN_MAX_SPEED_MPS = 17.88
+SIGN_MODEL_AGREE_M = 50.0
+SIGN_SEED_M = 27.0
+SIGN_APPROACH_DECEL = 1.0
+MODEL_APPROACH_DECEL = .65
+
 
 @dataclass(frozen=True)
 class StopTune:
@@ -47,6 +55,7 @@ class StopFrame:
   steering_angle_deg: float
   resume_requested: bool = False
   perception_available: bool = True
+  sign: bool = False
 
 
 @dataclass(frozen=True)
@@ -81,13 +90,16 @@ class ForceStop:
     self.activation_gate = False
     self.stop_seen_ns = 0
     self.standstill_since_ns = 0
+    self.standstill_clear_ns = 0
     self.standstill_reason = None
+    self.sign_confirmed = False
     self.previously_enabled = False
     self.distance = 0.0
     self.distance_cap = 0.0
 
   def _clear_standstill(self):
     self.standstill_since_ns = 0
+    self.standstill_clear_ns = 0
     self.standstill_reason = None
 
   def step(self, frame: StopFrame, tune: StopTune, offset_ft: int = 0) -> StopPlan:
@@ -109,7 +121,7 @@ class ForceStop:
       self.reset()
       return StopPlan()
     available = frame.perception_available
-    if available and (frame.light or frame.raw_model_stopped):
+    if available and (frame.light or frame.raw_model_stopped) or frame.sign:
       if not frame.standstill:
         self.stop_seen_ns = now
     if frame.standstill:
@@ -123,7 +135,10 @@ class ForceStop:
     permitted = (available and now >= self.override_until_ns and not frame.driving_in_curve and not turn and
                  not frame.tracking_lead and not lead)
     model_path = frame.light and model_active and permitted and not curved
-    active = model_path
+    sign_path = frame.sign and permitted and speed < SIGN_MAX_SPEED_MPS and horizon < SIGN_MODEL_AGREE_M
+    if sign_path:
+      self.sign_confirmed = True
+    active = model_path or sign_path
     if model_path:
       self.from_light = True
       self.light_clear_ns = 0
@@ -131,39 +146,49 @@ class ForceStop:
       self.from_light = False
       self.light_clear_ns = 0
     stop_scene = available and (active or frame.raw_model_stopped)
-    # Reopened geometry may withdraw a moving approach; it cannot classify
-    # a stationary hold as a green light.
     go_scene = available and horizon >= 50.0 and not frame.raw_model_stopped and not frame.model_should_stop
     held_before = self.previously_enabled and self.standstill_reason is not None
     engaged_stopped = not self.previously_enabled and frame.standstill
-    if frame.standstill and (self.forcing or engaged_stopped and stop_scene) and self.standstill_reason is None:
+    if frame.standstill and self.sign_confirmed and self.standstill_reason != 'sign':
+      self.standstill_since_ns = now
+      self.standstill_reason = 'sign'
+      self.distance = 0
+    elif frame.standstill and (self.forcing or engaged_stopped and stop_scene) and self.standstill_reason is None:
       self.standstill_since_ns = now
       self.standstill_reason = 'unclassified'
       self.distance = 0
+    if self.standstill_reason is not None:
+      if not available or stop_scene:
+        self.standstill_clear_ns = 0
+      elif not self.standstill_clear_ns:
+        self.standstill_clear_ns = now
+    hold_expired = self.standstill_reason not in (None, 'sign') and (
+      now - self.standstill_since_ns >= STANDSTILL_HOLD_NS or
+      self.standstill_clear_ns > 0 and now - self.standstill_clear_ns >= STANDSTILL_CLEAR_NS)
     if held_before and frame.standstill and frame.resume_requested:
       self.resume_until_ns = now + 6_000_000_000
     if abs(speed) >= 2.0 or frame.lead_present or frame.tracking_lead:
       self.resume_until_ns = 0
     driver_release = frame.pedal_override or frame.resume_requested
-    if self.standstill_reason is not None and driver_release:
+    if self.standstill_reason is not None and (driver_release or hold_expired):
       self.override_until_ns = now + 10_000_000_000
       self._clear_standstill()
     held = self.standstill_reason is not None
     if not available and not self.forcing:
       self.timer = 0
     elif active and not frame.standstill:
-      self.timer = min(2.0, self.timer + DT_MDL)
+      self.timer = min(2.0, self.timer + DT_MDL * (2 if frame.sign else 1))
     elif turn and not self.forcing and not frame.standstill:
       self.timer = 0
     elif held:
       self.timer = max(self.timer, .5)
-    elif self.forcing and frame.standstill and not frame.light and not frame.raw_model_stopped:
+    elif self.forcing and frame.standstill and not frame.sign and not frame.light and not frame.raw_model_stopped:
       self.timer = 0
     else:
       self.timer = max(0, self.timer - DT_MDL * .25)
     force = self.timer >= .5 or self.forcing and not frame.standstill or held
     light_cleared = (not held and self.forcing and self.from_light and not frame.standstill and
-                     available and not frame.light and go_scene)
+                     available and not frame.light and not frame.sign and go_scene)
     low_speed_hold = (light_cleared and tune.low_speed_hold_mps is not None and self.entry_speed is not None and
                       speed <= tune.low_speed_hold_mps and speed < self.entry_speed - .25)
     if light_cleared and not low_speed_hold:
@@ -192,7 +217,7 @@ class ForceStop:
         ceiling = 0.0
       else:
         self.distance = max(0.0, self.distance - speed * dt)
-        if (available and self.distance > max(tune.handoff_m, 40) and not frame.model_should_stop and
+        if (available and not frame.sign and self.distance > max(tune.handoff_m, 40) and not frame.model_should_stop and
             horizon > self.distance + 3 and (tune.reanchor_speed_tolerance is None or self.entry_speed is None or
                                             speed >= self.entry_speed - tune.reanchor_speed_tolerance)):
           self.distance = horizon
@@ -200,18 +225,25 @@ class ForceStop:
           self.distance = min(self.distance, horizon)
         self.distance_cap = max(0.0, self.distance_cap - speed * dt)
         self.distance = min(self.distance, self.distance_cap + 15 * min(self.distance_cap / 60, 1))
+        if frame.sign and available:
+          if horizon < SIGN_MODEL_AGREE_M:
+            self.distance = min(self.distance, SIGN_SEED_M)
+          if horizon < SIGN_SEED_M:
+            self.distance = horizon
         effective = self.distance + offset + tune.distance_bias_m
-        ceiling = math.sqrt(2 * .65 * max(0, effective - tune.handoff_m))
+        decel = SIGN_APPROACH_DECEL if frame.sign else MODEL_APPROACH_DECEL
+        ceiling = math.sqrt(2 * decel * max(0, effective - tune.handoff_m))
     else:
       self.forcing = False
       self.entry_speed = None
+      self.sign_confirmed = False
       self._clear_standstill()
       self.distance = self.distance_cap = horizon if available else 0.0
       if frame.light and permitted and not curved and not overridden:
         approach = horizon
         effective = approach + offset + tune.distance_bias_m
         if effective > tune.handoff_m:
-          ceiling = math.sqrt(2 * .65 * (effective - tune.handoff_m))
+          ceiling = math.sqrt(2 * MODEL_APPROACH_DECEL * (effective - tune.handoff_m))
     self.previously_enabled = frame.enabled
     stop_length = self.distance if self.forcing and self.distance > tune.handoff_m else approach
     # The MPC obstacle includes its standing-distance term; adding it at the

openpilot/starpilot/longitudinal/force_stop_runtime.py

@@ -33,6 +33,17 @@ def standstill_takeoff_light(model, car) -> bool | None:
   return should_stop or horizon < FROZEN_RAW_STOP_DISTANCE_M + STOP_MODEL_RELEASE_MARGIN_M
 
 
+def dashboard_stop_sign(sm, *, now_ns: int, drive_id: int) -> bool:
+  try:
+    if not sm.seen['starpilotCarState'] or not sm.valid['starpilotCarState']:
+      return False
+    stamp = int(sm.logMonoTime['starpilotCarState'])
+    return bool(drive_id < stamp <= now_ns and now_ns - stamp <= SOURCE_AGE_NS and
+                sm['starpilotCarState'].dashboardStopSign > 0)
+  except (AttributeError, KeyError, TypeError, ValueError):
+    return False
+
+
 class ForceStopRuntime:
   def __init__(self, params):
     self.params = params
@@ -173,7 +184,8 @@ class ForceStopRuntime:
                         tracked is not None and tracked.tracked is True, bool(lead.present), float(lead.dRel), float(lead.vLead),
                         raw_curve[1] if raw_curve is not None else False, curvature[0] if curvature is not None else None,
                         bool(car.leftBlinker or car.rightBlinker), float(car.steeringAngleDeg), resume_requested=resume_requested,
-                        perception_available=perception_available)
+                        perception_available=perception_available,
+                        sign=dashboard_stop_sign(sm, now_ns=now_ns, drive_id=drive_id))
       self.plan = self.policy.step(frame, vehicle_tune(str(cp.carFingerprint)), self.offset)
       if takeoff_enabled:
         self.plan = replace(self.plan, takeoff_light_observed=takeoff_observed, takeoff_light=takeoff_light)

openpilot/starpilot/longitudinal/profile_runtime.py

@@ -8,9 +8,11 @@ from dataclasses import astuple, dataclass
 from openpilot.common.constants import CV
 from opendbc.car.structs import car
 from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.starpilot.longitudinal import drive_mode as drive_mode_profiles
 from openpilot.starpilot.longitudinal.accel_profile import A_CRUISE_MAX_VALS_TRAFFIC_ALL, interpolate_accel_profile
 from openpilot.starpilot.longitudinal.profile_document import (
-  DEFAULT_DECELERATION_PROFILE, active_personality_id, interpolate_category_curve, is_truck_fingerprint,
+  DEFAULT_DECELERATION_PROFILE, active_personality_id, default_personality_profiles, interpolate_category_curve,
+  is_truck_fingerprint, profile_document,
 )
 from openpilot.starpilot.longitudinal.profile_preferences import (
   FOLLOW_MIN_SECONDS, FOLLOW_MAX_SECONDS, NAMES, SCALARS, TRAFFIC_JERK_SUFFIXES,
@@ -368,6 +370,7 @@ class ProfileHost:
     self.disabled = False
     self.selected_document: dict | None = None
     self.selected_powertrain: GlobalPowertrainPreset | None = None
+    self.selected_drive_mode = (False, False)
     self.selected_attempt_ns = -self.REFRESH_NS
     self.selected_success_ns = -1
     self.settings: ProfileSettings | TrafficSettings | None = None
@@ -380,10 +383,11 @@ class ProfileHost:
     self.global_last_success_ns = -1
 
   def sample_selected(self, now_ns: int, personality, v_ego: float, CP, *, traffic_mode: bool | None = False,
-                      legacy: ProfileTuning | None = None) -> SelectedProfileTuning | None:
+                      legacy: ProfileTuning | None = None, drive_mode: str | None = None) -> SelectedProfileTuning | None:
     if now_ns < 0 or now_ns < self.selected_attempt_ns:
       self.selected_document = None
       self.selected_powertrain = None
+      self.selected_drive_mode = (False, False)
       self.selected_attempt_ns = now_ns - self.REFRESH_NS
       self.selected_success_ns = -1
     if now_ns - self.selected_attempt_ns >= self.REFRESH_NS:
@@ -392,14 +396,22 @@ class ProfileHost:
         saved = read_document_value(self.params)
         self.selected_document = saved.value if saved.valid and isinstance(saved.value, dict) else None
         self.selected_powertrain = read_global_powertrain_preset(self.params, CP)
+        self.selected_drive_mode = drive_mode_profiles.read_enabled(self.params)
         self.selected_success_ns = now_ns if saved.valid else -1
       except (OSError, RuntimeError, ValueError, TypeError, KeyError):
         self.selected_document = None
         self.selected_powertrain = None
+        self.selected_drive_mode = (False, False)
         self.selected_success_ns = -1
     if self.selected_success_ns < 0 or now_ns - self.selected_success_ns > self.MAX_AGE_NS:
       return None
-    return resolve_selected_profiles(self.selected_document, personality, v_ego, CP,
+    document = self.selected_document
+    if any(self.selected_drive_mode) and traffic_mode is False and drive_mode in drive_mode_profiles.PROFILES:
+      if document is None:
+        document = profile_document(default_personality_profiles(False), enabled=False)
+      document = drive_mode_profiles.apply(document, drive_mode, acceleration=self.selected_drive_mode[0],
+                                           deceleration=self.selected_drive_mode[1])
+    return resolve_selected_profiles(document, personality, v_ego, CP,
                                      traffic_mode=traffic_mode, legacy=legacy, global_powertrain=self.selected_powertrain)
 
   def sample_global_braking(self, now_ns: int) -> str | None:

openpilot/starpilot/longitudinal/tests/test_force_stop.py

@@ -109,14 +109,13 @@ class ForceStopTests(unittest.TestCase):
     held = self.owner.step(frame(35, speed_mps=0., standstill=True, horizon_m=None,
                                  road_curvature=None, light=False, perception_available=False), self.tune)
     self.assertTrue(held.should_stop)
-    for tick in range(36, 76):
+    for tick in range(36, 75):
       self.assertTrue(self.owner.step(frame(tick, speed_mps=-1e-44, standstill=True, horizon_m=None,
                                           road_curvature=None, light=False, perception_available=False), self.tune).should_stop)
-    # Reopened geometry cannot classify a held stop or authorize departure.
-    for tick in range(76, 92):
-      plan = self.owner.step(frame(tick, speed_mps=0., standstill=True, horizon_m=190., light=False), self.tune)
-    self.assertTrue(plan.forcing)
-    self.assertTrue(plan.manual_hold)
+    expired = self.owner.step(frame(75, speed_mps=0., standstill=True, horizon_m=None,
+                                    road_curvature=None, light=False, perception_available=False), self.tune)
+    self.assertFalse(expired.forcing)
+    self.assertFalse(expired.manual_hold)
 
   def test_missing_perception_distance_uses_actual_model_interval(self):
     for tick in range(15):
@@ -152,39 +151,84 @@ class ForceStopTests(unittest.TestCase):
       self.assertFalse(plan.forcing)
       self.assertIsNone(plan.speed_ceiling_mps)
 
-  def test_unclassified_standstill_holds_until_driver_release(self):
-    for tick in range(80):
+  def test_standstill_hold_expires_after_two_seconds_without_rearming(self):
+    for tick in range(40):
       plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=30., raw_model_stopped=True, standstill=True), self.tune)
       self.assertTrue(plan.forcing)
       self.assertTrue(plan.should_stop)
-    for tick in range(80, 96):
-      plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., raw_model_stopped=False,
-                                  light=False, standstill=True), self.tune)
-    self.assertTrue(plan.forcing)
-    self.assertTrue(plan.should_stop)
-    self.assertTrue(plan.manual_hold)
-    for action in ('resume_requested', 'pedal_override'):
-      released = self.owner.step(frame(96 if action == 'resume_requested' else 97, speed_mps=0.,
-                                horizon_m=190., light=False, standstill=True, **{action: True}), self.tune)
-      self.assertFalse(released.manual_hold)
-      self.assertFalse(released.forcing)
-      self.assertFalse(released.should_stop)
+      self.assertTrue(plan.manual_hold)
+    for tick in range(40, 240):
+      plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=30., raw_model_stopped=True, standstill=True), self.tune)
+      self.assertFalse(plan.forcing)
+      self.assertFalse(plan.should_stop)
+      self.assertFalse(plan.manual_hold)
 
-  def test_manual_hold_survives_clear_geometry_and_roll_until_each_driver_action(self):
+  def test_moving_stop_hold_expires_after_two_seconds(self):
+    for tick in range(15):
+      self.owner.step(frame(tick, horizon_m=30.), self.tune)
+    for tick in range(15, 55):
+      plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=30., standstill=True), self.tune)
+      self.assertTrue(plan.manual_hold)
+    plan = self.owner.step(frame(55, speed_mps=0., horizon_m=30., standstill=True), self.tune)
+    self.assertFalse(plan.forcing)
+    self.assertFalse(plan.should_stop)
+
+  def test_standstill_hold_releases_after_open_scene_dwell(self):
+    self.assertTrue(self.owner.step(frame(0, speed_mps=0., horizon_m=30., raw_model_stopped=True,
+                                         standstill=True), self.tune).manual_hold)
+    for tick in range(1, 16):
+      plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., light=False, standstill=True), self.tune)
+      self.assertTrue(plan.manual_hold)
+    plan = self.owner.step(frame(16, speed_mps=0., horizon_m=190., light=False, standstill=True), self.tune)
+    self.assertFalse(plan.manual_hold)
+    self.assertFalse(plan.forcing)
+    self.assertFalse(plan.should_stop)
+
+  def test_manual_hold_survives_roll_until_each_driver_action(self):
     for action in ('resume_requested', 'pedal_override'):
       self.owner.reset()
       held = self.owner.step(frame(0, speed_mps=0., horizon_m=30., raw_model_stopped=True, standstill=True), self.tune)
       self.assertTrue(held.manual_hold)
-      for tick in range(1, 401):
-        plan = self.owner.step(frame(tick, speed_mps=.01, horizon_m=190., light=False, standstill=False), self.tune)
+      for tick in range(1, 31):
+        plan = self.owner.step(frame(tick, speed_mps=.01, horizon_m=30., raw_model_stopped=True, standstill=False), self.tune)
         self.assertTrue(plan.manual_hold)
         self.assertEqual(plan.speed_ceiling_mps, 0.)
-      release = self.owner.step(frame(401, speed_mps=0., horizon_m=190., light=False, standstill=True,
+      release = self.owner.step(frame(31, speed_mps=0., horizon_m=30., raw_model_stopped=True, standstill=True,
                                      **{action: True}), self.tune)
       self.assertFalse(release.manual_hold)
       self.assertFalse(release.forcing)
       self.assertFalse(release.should_stop)
 
+  def test_dashboard_sign_arms_only_slow_and_with_model_agreement(self):
+    for changes, armed in (({}, True), ({'speed_mps': 18.5}, False), ({'horizon_m': 60.}, False),
+                           ({'tracking_lead': True}, False)):
+      with self.subTest(changes=changes):
+        self.owner.reset()
+        for tick in range(6):
+          plan = self.owner.step(frame(tick, **{'speed_mps': 8., 'horizon_m': 45., 'light': False, 'sign': True, **changes}),
+                                 self.tune)
+        self.assertEqual(plan.forcing, armed)
+        if armed:
+          self.assertLessEqual(plan.tracked_distance_m, 27.)
+
+  def test_dashboard_sign_hold_waits_for_each_driver_action(self):
+    for action in ('pedal_override', 'resume_requested'):
+      with self.subTest(action=action):
+        self.owner.reset()
+        for tick in range(15):
+          self.owner.step(frame(tick, speed_mps=8., horizon_m=45., light=False, sign=True), self.tune)
+        for tick in range(15, 415):
+          plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., light=False, standstill=True), self.tune)
+          self.assertTrue(plan.manual_hold)
+          self.assertTrue(plan.should_stop)
+        released = self.owner.step(frame(415, speed_mps=0., horizon_m=190., light=False, standstill=True,
+                                         **{action: True}), self.tune)
+        self.assertFalse(released.manual_hold)
+        self.assertFalse(released.forcing)
+        for tick in range(416, 600):
+          plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., light=False, standstill=True), self.tune)
+          self.assertFalse(plan.forcing)
+
   def test_odometry_ceiling_bounds_reopened_model(self):
     for tick in range(15):
       self.owner.step(frame(tick), self.tune)

openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py

@@ -10,7 +10,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.plannerd import update_curve_frame
 from openpilot.starpilot.curve_speed.host import CurveHost
 from openpilot.starpilot.longitudinal.force_stop import StopPlan
-from openpilot.starpilot.longitudinal.force_stop_runtime import ForceStopRuntime
+from openpilot.starpilot.longitudinal.force_stop_runtime import ForceStopRuntime, dashboard_stop_sign
 from openpilot.starpilot.longitudinal.tests.test_cruise_ceiling import messages, snapshot
 from openpilot.starpilot.longitudinal.tests.test_stop_resume import event as button_event
 
@@ -139,15 +139,13 @@ class ForceStopRuntimeTests(unittest.TestCase):
       plan = self.sample(sm)
       self.assertTrue(plan.should_stop)
       self.assertEqual(plan.speed_ceiling_mps, 0.)
-    for tick in range(100, 116):
-      plan = self.sample(Frame(tick, horizon=190., stopped=True))
     self.assertTrue(plan.forcing)
     self.assertTrue(plan.manual_hold)
-    now = BASE + 5_800_000_000 + 1_000_000
-    for item in (button_event(577), button_event(578, [('resumeCruise', True)]),
-                 button_event(579, [('resumeCruise', False)])):
+    now = BASE + 5_000_000_000 + 1_000_000
+    for item in (button_event(497), button_event(498, [('resumeCruise', True)]),
+                 button_event(499, [('resumeCruise', False)])):
       self.owner.resume.observe(item, now_ns=now, drive_id=DRIVE)
-    released = self.sample(Frame(116, horizon=190., stopped=True))
+    released = self.sample(Frame(100, horizon=190., stopped=True))
     self.assertFalse(released.manual_hold)
     self.assertFalse(released.forcing)
     self.assertIsNone(released.speed_ceiling_mps)
@@ -219,13 +217,14 @@ class ForceStopRuntimeTests(unittest.TestCase):
     self.assertTrue(output.forcing)
     self.assertTrue(output.should_stop)
     self.assertEqual(output.speed_ceiling_mps, 0.)
-    for tick in range(41, 101):
+    for tick in range(41, 80):
       output = self.sample(Frame(tick, stopped=True, horizon=30.))
       self.assertTrue(output.should_stop)
-    for tick in range(101, 201):
-      output = self.sample(Frame(tick, stopped=True, horizon=192.))
-    self.assertTrue(output.forcing)
-    self.assertTrue(output.manual_hold)
+      self.assertTrue(output.manual_hold)
+    for tick in range(80, 200):
+      output = self.sample(Frame(tick, stopped=True, horizon=30.))
+      self.assertFalse(output.forcing)
+      self.assertFalse(output.manual_hold)
 
   def test_manual_hold_publishes_dedicated_bit_and_release_withdraws_same_cycle(self):
     from types import SimpleNamespace
@@ -361,6 +360,21 @@ class ForceStopRuntimeTests(unittest.TestCase):
         self.assertEqual(snapshot(original), snapshot(proposed))
         np.testing.assert_array_equal(original.mpc.params, proposed.mpc.params)
 
+  def test_dashboard_stop_sign_requires_fresh_valid_companion(self):
+    from openpilot.cereal import messaging
+    message = messaging.new_message('starpilotCarState')
+    message.starpilotCarState.dashboardStopSign = 1
+    for defect, expected in ((None, True), ('stale', False), ('invalid', False), ('unseen', False), ('missing', False)):
+      with self.subTest(defect=defect):
+        sm = Frame(0)
+        if defect != 'missing':
+          sm['starpilotCarState'] = message.starpilotCarState
+          sm.logMonoTime['starpilotCarState'] = sm.stamp
+          sm.valid['starpilotCarState'] = defect != 'invalid'
+        sm.seen = {'starpilotCarState': defect not in ('unseen', 'missing')}
+        now = sm.stamp + (150_000_001 if defect == 'stale' else 1_000_000)
+        self.assertEqual(dashboard_stop_sign(sm, now_ns=now, drive_id=DRIVE), expected)
+
 
 if __name__ == '__main__':
   unittest.main()

openpilot/starpilot/ui/feature_settings_owner.py

@@ -17,6 +17,7 @@ from openpilot.starpilot.lateral.pause import KEYS as LATERAL_PAUSE_KEYS
 from openpilot.starpilot.lateral.lane_centering import STRENGTH_BASELINE, STRENGTH_MAX
 
 from opendbc.car.structs import car
+from openpilot.common.hardware import HARDWARE
 from openpilot.common.params import UnknownKeyName
 from openpilot.starpilot.longitudinal.accel_profile import A_CRUISE_MAX_VALS_TRAFFIC_ALL, interpolate_accel_profile
 from openpilot.starpilot.longitudinal.profile_runtime import TRAFFIC_CRUISE_BRAKE_MAGNITUDE
@@ -74,10 +75,16 @@ from openpilot.starpilot.curve_speed.actions import ACTION_KEYS as CURVE_ACTION_
 from openpilot.starpilot.controllers.toyota_cruise import capability as toyota_cruise_capability
 
 
-GLOBAL_BOOL_PREFERENCES = frozenset((LEAD_APPROACH_KEY, LEAD_TAKEOFF_KEY, NUDGE_KEY, "DisengageOnAccelerator"))
+AGGRESSIVE_COOLING_KEY = "AggressiveCoolingEnabled"
+AGGRESSIVE_COOLING_DESCRIPTION = ("Runs the fan sooner and harder to keep the device 10-16 °C cooler on hot days, " +
+                                  "at the cost of more fan noise.")
+GLOBAL_BOOL_PREFERENCES = frozenset((LEAD_APPROACH_KEY, LEAD_TAKEOFF_KEY, NUDGE_KEY, "DisengageOnAccelerator", AGGRESSIVE_COOLING_KEY))
+
+DRIVE_MODE_KEYS = ("DriveModeProfiles", "DriveModeAcceleration", "DriveModeDeceleration")
 
 BOOL_DEFAULTS = {
-  "GMPedalLongitudinal": False, "ForceStops": False, "AlwaysAllowUploads": False, "TurnAssist": True,
+  "GMPedalLongitudinal": False, "ForceStops": False, "AlwaysAllowUploads": False, "TurnAssist": True, "HighwaySmoothing": False,
+  "DriveModeProfiles": False, "DriveModeAcceleration": True, "DriveModeDeceleration": True,
   "SubaruAvhStartup": False,
   "SubaruStopStartOff": False,
   "SubaruSNG": False,
@@ -93,6 +100,7 @@ BOOL_DEFAULTS = {
   LEAD_TAKEOFF_KEY: False,
   NUDGE_KEY: True,
   "DisengageOnAccelerator": False,
+  AGGRESSIVE_COOLING_KEY: False,
   CURVE_MASTER_KEY: False, CURVE_NO_LEAD_KEY: False,
   "ShowCSCStatus": False,
   "ForceAutoTuneOff": False, "AlwaysOnLateral": True, "NostalgiaMode": False,
@@ -806,6 +814,70 @@ class FeatureSettingsOwner:
                           authorized=authorized, temp_prefix=".turn-assist-")
     return result.committed and result.verified
 
+  def _drive_mode_capable(self) -> bool:
+    from opendbc.car.hyundai.values import HyundaiFlags
+    cp = self.vehicle_params()
+    return (cp is not None and cp.brand == "hyundai" and not cp.notCar and
+            bool(int(cp.flags) & HyundaiFlags.CANFD) and bool(int(cp.flags) & HyundaiFlags.EV))
+
+  def _highway_smoothing_capability(self) -> tuple | None:
+    from openpilot.starpilot.lateral.controller_selection import policy_for
+    cp = self.vehicle_params()
+    if cp is None or policy_for(cp) != 'ioniq6':
+      return None
+    return (cp.carFingerprint, cp.lateralTuning.which(), str(cp.steerControlType), cp.passive, cp.dashcamOnly, cp.notCar,
+            tuple((str(config.safetyModel), int(config.safetyParam)) for config in cp.safetyConfigs))
+
+  def _highway_smoothing_rows(self, configurable: bool) -> list[FeatureRow]:
+    from openpilot.selfdrive.controls.lib.highway_correction_gain import DEFAULT_GAIN, ENABLE_KEY, GAIN_KEY, MIN_GAIN
+    capability = self._highway_smoothing_capability()
+    allowed = configurable and self.authority("torque") and capability is not None
+    toggle = self._bool_row(ENABLE_KEY, "Highway Smoothing", allowed)
+    rows = [replace(toggle, capability=capability, vehicle_fingerprint=self.vehicle_fingerprint(), dependencies=(),
+                    reason=toggle.reason if toggle.value not in ("On", "Off") else
+                           ("Calms highway weave on straights and steady curves above 35 mph. Applies within one second."
+                            if capability is not None else "Highway Smoothing is available on the Ioniq 6"))]
+    if toggle.value != "On":
+      return rows
+    value, raw, valid = self._value(GAIN_KEY)
+    try:
+      number = DEFAULT_GAIN if raw is None else float(value)
+      valid = valid and self._readable(GAIN_KEY) and math.isfinite(number) and MIN_GAIN <= number <= 1.0
+    except ValueError:
+      valid, number = False, DEFAULT_GAIN
+    rows.append(FeatureRow(GAIN_KEY, "Strength", f"{number:.2f}" if valid else "Invalid saved value", raw,
+                           step=0.05 if valid else 0., minimum=MIN_GAIN, maximum=1.0,
+                           available=allowed and (valid or self.authority("parked_preferences") and self._readable(GAIN_KEY)),
+                           repair_value=str(DEFAULT_GAIN) if not valid else "", default_value=str(DEFAULT_GAIN),
+                           vehicle_fingerprint=self.vehicle_fingerprint(), capability=capability,
+                           reason="Lower values follow the model's quick back-and-forth corrections less. 1.00 matches off."))
+    return rows
+
+  def _apply_highway_smoothing(self, request: FeatureSettingsRequest) -> bool:
+    from openpilot.selfdrive.controls.lib.highway_correction_gain import ENABLE_KEY, MIN_GAIN
+    def authorized():
+      row = next((row for row in self._highway_smoothing_rows(self.authority("preferences")) if row.key == request.key), None)
+      return (row is not None and row.available and (not row.repair_value or request.value == row.repair_value) and
+              row.capability == request.capability and row.vehicle_fingerprint == request.vehicle_fingerprint and
+              row.source == request.expected and request.related_source is None and not request.direction)
+    if not authorized():
+      return False
+    if request.key == ENABLE_KEY:
+      if request.value not in ("Off", "On"):
+        return False
+      raw = b"1" if request.value == "On" else b"0"
+    else:
+      try:
+        number = float(request.value)
+      except ValueError:
+        return False
+      if not math.isfinite(number) or not MIN_GAIN <= number <= 1.0:
+        return False
+      raw = str(number).encode()
+    result = commit_exact(self.params, key=request.key, max_bytes=128, raw=raw, expected=request.expected,
+                          authorized=authorized, temp_prefix=".highway-smoothing-")
+    return result.committed and result.verified
+
   def snapshot(self, page: str, *, parked: bool, system_long: bool, lateral_context: bool, metric: bool,
                configure_while_driving: bool = False) -> FeatureSettingsState:
     selected_vehicle = self._vehicle_params_source()
@@ -828,6 +900,11 @@ class FeatureSettingsOwner:
       title = "Data Uploads"
       rows = [self._bool_row("AlwaysAllowUploads", "Always Allow Uploads", self.authority("preferences"),
                              "Allow queued full logs and videos on metered networks. Uses mobile data.")]
+    elif page == "cooling":
+      title = "Cooling"
+      if HARDWARE.get_device_type() in ("tici", "tizi"):
+        rows = [self._bool_row(AGGRESSIVE_COOLING_KEY, "Aggressive Cooling", self.authority("preferences"),
+                               AGGRESSIVE_COOLING_DESCRIPTION)]
     elif page == FeaturePage.HUB:
       rows = [FeatureRow("", "Curve Speed Controller", "Adjust how speed is reduced for curves ahead", page=FeaturePage.CURVE, available=True),
               FeatureRow("", "Speed Limit Controller", "Choose speed-limit sources, offsets, and confirmation behavior", page=FeaturePage.SLC, available=True),
@@ -838,6 +915,10 @@ class FeatureSettingsOwner:
               FeatureRow("", "Steering and Torque", "Select the steering controller and tune its response", page=FeaturePage.TORQUE, available=True),
               FeatureRow("", "Always On Lateral", "Keep steering assistance active independently of cruise control", page=FeaturePage.AOL, available=True),
               FeatureRow("", "Wheel Controls", "Assign steering-wheel buttons and cruise behavior", page=FeaturePage.WHEEL, available=True)]
+      if self._highway_smoothing_capability() is not None:
+        rows.insert(rows.index(next(row for row in rows if row.page == FeaturePage.TORQUE)) + 1,
+                    FeatureRow("", "Highway Smoothing", "Calm highway weave on straights and steady curves",
+                               page=FeaturePage.HIGHWAY_SMOOTHING, available=True))
       if (self._long_pitch_capability() is not None or self._bolt_disable_capability() is not None or
           self._auto_hold_capability() is not None or self._toyota_sng_capability() is not None or self._jeep_hold_capability() is not None or
           self._toyota_radar_capability() is not None or self._pedal_setup_capability() is not None or
@@ -1047,6 +1128,9 @@ class FeatureSettingsOwner:
       if learning_row is not None:
         rows.append(learning_row)
       rows.extend(numeric_rows)
+    elif page == FeaturePage.HIGHWAY_SMOOTHING:
+      title = "Highway Smoothing"
+      rows.extend(self._highway_smoothing_rows(configurable))
     elif page == FeaturePage.AOL:
       title = "Always On Lateral"
       rows.extend(self._lateral_pause_rows(configurable))
@@ -1190,6 +1274,16 @@ class FeatureSettingsOwner:
       if self._galaxy():
         rows.append(self._bool_row("DisengageOnAccelerator", "Disengage on Accelerator Pedal", self.authority("preferences")))
       document, raw, valid = self._document()
+      if self._drive_mode_capable():
+        match_enabled, _, _ = self._value("DriveModeProfiles")
+        rows.append(self._bool_row("DriveModeProfiles", "Match Drive Mode", allowed,
+                                   reason="Eco, Normal and Sport select Eco, Standard and Sport+ acceleration with Eco, "
+                                          "Standard and Sport deceleration, replacing the selected profiles below. Traffic Mode is unchanged."))
+        if match_enabled == "1":
+          rows.append(self._bool_row("DriveModeAcceleration", "Match Acceleration", allowed,
+                                     reason="Eco, Normal and Sport select Eco, Standard and Sport+ acceleration."))
+          rows.append(self._bool_row("DriveModeDeceleration", "Match Deceleration", allowed,
+                                     reason="Eco, Normal and Sport select Eco, Standard and Sport deceleration."))
       for category, field, label, choices in (
         ('acceleration', 'selectedAccelerationProfile', 'Selected Acceleration Profile',
          ('dom_default', 'standard', 'eco', 'sport', 'sport_plus')),
@@ -1366,6 +1460,8 @@ class FeatureSettingsOwner:
       return FordBluePilotFeature(self).apply(request)
     if key == "TurnAssist":
       return self._apply_turn_assist(request)
+    if key in ("HighwaySmoothing", "HighwayCorrectionGain"):
+      return self._apply_highway_smoothing(request)
     if key == 'LateralControllerSelection':
       return self.controller.apply(request)
     if key == SETUP_ACTION:
@@ -1474,7 +1570,7 @@ class FeatureSettingsOwner:
       group = "long"
     elif key in BOOL_DEFAULTS or key in FLOAT_SPECS or key in (SLC_PRIORITY, SLC_SECONDARY, 'SLCAdvanceSeconds'):
       source_key = key
-      long_key = (key in ("CustomPersonalities", "ForceStops", "ForceStopDistanceOffset") or "Personality" in key or
+      long_key = (key in ("CustomPersonalities", "ForceStops", "ForceStopDistanceOffset", *DRIVE_MODE_KEYS) or "Personality" in key or
                   key.endswith(("Follow", "FollowHigh")) or "Jerk" in key)
       if key in ("ShowSpeedLimits", "AlwaysAllowUploads", SLC_PRIORITY, SLC_SECONDARY) or (key == "ForceStops" and request.value == "Off"):
         group = "preferences"

openpilot/starpilot/ui/feature_settings_state.py

@@ -59,6 +59,7 @@ class FeaturePage(StrEnum):
   TORQUE = "torque"
   BLUEPILOT = "torque/bluepilot"
   AOL = "aol"
+  HIGHWAY_SMOOTHING = "highway_smoothing"
   WHEEL = "wheel"
   CONDITIONAL = "conditional"
   CONDITIONAL_CEM = "conditional/cem"

openpilot/starpilot/ui/runtime_app.py

@@ -31,7 +31,7 @@ from openpilot.starpilot.drive_state.owner import DriveStateOwner, Rejected as D
 from openpilot.starpilot.drive_state.evidence import PhysicalSource
 from openpilot.starpilot.drive_state.control import DriveStateControl, LABELS
 from openpilot.starpilot.storage import starpilot_storage_root
-from openpilot.starpilot.ui.feature_settings_owner import FeatureSettingsOwner
+from openpilot.starpilot.ui.feature_settings_owner import AGGRESSIVE_COOLING_DESCRIPTION, AGGRESSIVE_COOLING_KEY, FeatureSettingsOwner
 from openpilot.starpilot.ui.conditional_feature import confirmation_question as conditional_question
 from openpilot.starpilot.ui.appearance_owner import AppearanceOwner
 from openpilot.starpilot.ui.appearance_preferences import CameraViewChoice
@@ -94,7 +94,7 @@ from openpilot.starpilot.ui.software_state import DownloadLabel, SoftwareAction,
 from openpilot.starpilot.ui.toggles_state import Personality, ToggleKey, ToggleRequest
 from openpilot.system.ui.lib.application import MouseEvent, MousePos, gui_app
 from openpilot.system.ui.widgets import Widget
-from openpilot.system.ui.widgets.list_view import button_item, text_item
+from openpilot.system.ui.widgets.list_view import button_item, text_item, toggle_item
 from openpilot.starpilot.ui.compact_settings_page import StarCompactSettings
 
 NATIVE_SETTINGS_PANELS = (Destination.DEVICE, Destination.SOFTWARE, Destination.BLUETOOTH, Destination.TOGGLES, Destination.DEVELOPER)
@@ -1845,6 +1845,8 @@ class StarMainLayout(MainLayout):
     toggle_items = self._large_panels[Destination.TOGGLES]._scroller._items
     toggle_items.insert(2, text_item("Safe Mode", "N/A"))
     from openpilot.common.hardware import HARDWARE
+    if HARDWARE.get_device_type() in ("tici", "tizi"):
+      self._add_aggressive_cooling(self._large_panels[Destination.DEVICE])
     from openpilot.starpilot.ui.starpilot_settings_adapter_large import StarPilotSettingsAdapterLarge
     for destination, owner in self._large_panels.items():
       if destination == Destination.DEVELOPER and HARDWARE.get_device_type() in ('mici', 'tici', 'tizi'):
@@ -1861,6 +1863,19 @@ class StarMainLayout(MainLayout):
     self.star._native_favorite_actions = self._native_favorites
     self.star._publish_bookmark = self._on_bookmark_clicked
 
+  @staticmethod
+  def _add_aggressive_cooling(device_panel) -> None:
+    item = toggle_item("Aggressive Cooling", AGGRESSIVE_COOLING_DESCRIPTION,
+                       initial_state=ui_state.params.get_bool(AGGRESSIVE_COOLING_KEY),
+                       callback=lambda state: ui_state.params.put_bool(AGGRESSIVE_COOLING_KEY, state, block=True))
+    device_panel._scroller.add_widget(item)
+    native_show = device_panel.show_event
+
+    def show_event():
+      item.action_item.set_state(ui_state.params.get_bool(AGGRESSIVE_COOLING_KEY))
+      native_show()
+    device_panel.show_event = show_event
+
   def _exit_spacepilot(self) -> None:
     gui_app.pop_widgets_to(self, instant=True)
     self._set_mode_for_state()

openpilot/system/hardware/fan_controller.py

@@ -4,25 +4,36 @@ import numpy as np
 from openpilot.common.pid import PIDController
 from openpilot.common.hardware import HARDWARE
 
+IS_MICI = HARDWARE.get_device_type() == "mici"
+
 # raise fan setpoint on tici/tizi to reduce noise
 # after raising LMH threshold in AGNOS 18.1 to prevent CPU throttling
-OFFSET = 0 if HARDWARE.get_device_type() == "mici" else 5
+STOCK_CURVE = dict(offset=0, k_p=0., ff_low=60., ff_high=100.) if IS_MICI else \
+              dict(offset=5, k_p=0., ff_low=65., ff_high=105.)
+# tici/tizi opt-in: runs 10-16 C cooler than stock at the cost of fan noise
+AGGRESSIVE_CURVE = dict(offset=-5, k_p=1., ff_low=55., ff_high=80.)
 
 
 class FanController:
   def __init__(self, rate: int) -> None:
     self.last_ignition = False
-    self.controller = PIDController(k_p=0, k_i=4e-3, rate=rate)
+    self.last_aggressive = False
+    self.controller = PIDController(k_p=STOCK_CURVE["k_p"], k_i=4e-3, rate=rate)
+
+  def update(self, cur_temp: float, ignition: bool, aggressive_cooling: bool = False) -> int:
+    aggressive = aggressive_cooling and not IS_MICI
+    curve = AGGRESSIVE_CURVE if aggressive else STOCK_CURVE
 
-  def update(self, cur_temp: float, ignition: bool) -> int:
     self.controller.pos_limit = 100 if ignition else 30
     self.controller.neg_limit = 30 if ignition else 0
+    self.controller._k_p = ([0], [curve["k_p"]])
 
-    if ignition != self.last_ignition:
+    if ignition != self.last_ignition or aggressive != self.last_aggressive:
       self.controller.reset()
     self.last_ignition = ignition
+    self.last_aggressive = aggressive
 
     return int(self.controller.update(
-                 error=(cur_temp - (75 + OFFSET)),  # temperature setpoint in C
-                 feedforward=np.interp(cur_temp, [60.0 + OFFSET, 100.0 + OFFSET], [0, 100])
+                 error=(cur_temp - (75 + curve["offset"])),  # temperature setpoint in C
+                 feedforward=np.interp(cur_temp, [curve["ff_low"], curve["ff_high"]], [0, 100])
               ))

openpilot/system/hardware/hardwared.py

@@ -375,7 +375,8 @@ def hardware_thread(end_event, hw_queue) -> None:
     all_comp_temp = all_temp_filter.update(max(temp_sources))
     msg.deviceState.maxTempC = all_comp_temp
 
-    msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"])
+    msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"],
+                                                                   params.get_bool("AggressiveCoolingEnabled"))
 
     is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (time.monotonic() - off_ts > 60 * 5))
     if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP:

panda/board/obj/gitversion.h

@@ -1,4 +1,4 @@
 #pragma once
 
 extern const uint8_t gitversion[29];
-const uint8_t gitversion[29] = "DEV-e7cca0a1-Pa5919516-DEBUG";
+const uint8_t gitversion[29] = "DEV-4622fbb2-Pa5919516-DEBUG";