Fix Capn Proto image version detection

Model Manager allows for local driving models.

Compiler process for adding local driving models faster

.github/workflows/auto-cache/action.yaml

@@ -34,7 +34,7 @@ runs:
     - name: setup github cache
       id: gha-cache
       if: ${{ !contains(runner.name, 'nsc') && inputs.save != 'false' }}
-      uses: 'actions/cache@v4'
+      uses: 'actions/cache@v5'
       with:
         path: ${{ inputs.path }}
         key: ${{ inputs.key }}
@@ -43,7 +43,7 @@ runs:
     - name: setup github cache
       id: gha-cache-ro
       if: ${{ !contains(runner.name, 'nsc') && inputs.save == 'false' }}
-      uses: 'actions/cache/restore@v4'
+      uses: 'actions/cache/restore@v5'
       with:
         path: ${{ inputs.path }}
         key: ${{ inputs.key }}

.github/workflows/compile-openpilot/action.yaml

@@ -14,7 +14,7 @@ runs:
           ${{ env.RUN }} "rm -rf /tmp/scons_cache/* && \
                           scons -j$(nproc) --cache-populate"
     - name: Save scons cache
-      uses: actions/cache/save@v4
+      uses: actions/cache/save@v5
       if: github.ref == 'refs/heads/master'
       with:
         path: .ci_cache/scons_cache

.github/workflows/compile_bluescreens.yaml

@@ -88,7 +88,7 @@ jobs:
           fi
 
       - name: Checkout target branch
-        uses: actions/checkout@v4
+        uses: actions/checkout@v6
         with:
           ref: ${{ env.BUILD_BRANCH }}
           fetch-depth: 0

.github/workflows/compile_starpilot.yaml

@@ -48,7 +48,7 @@ jobs:
           fi
 
       - name: Checkout target branch
-        uses: actions/checkout@v4
+        uses: actions/checkout@v6
         with:
           ref: ${{ env.BUILD_BRANCH }}
           fetch-depth: 0

.github/workflows/publish_truenas_runner.yaml

@@ -17,17 +17,17 @@ jobs:
   publish:
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v6
 
       - name: Log in to GHCR
-        uses: docker/login-action@v3
+        uses: docker/login-action@v4
         with:
           registry: ghcr.io
           username: ${{ github.actor }}
           password: ${{ secrets.GITHUB_TOKEN }}
 
       - name: Build and push runner image
-        uses: docker/build-push-action@v6
+        uses: docker/build-push-action@v7
         with:
           context: .
           file: tools/truenas_github_runner/Dockerfile

cereal/custom.capnp

@@ -224,6 +224,16 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
 struct StarPilotRadarState @0xb86e6369214c01c8 {
   leadLeft @0 :LeadData;
   leadRight @1 :LeadData;
+  adjacentStopped @2 :AdjacentStopped;
+
+  # A vehicle in an adjacent lane that was observed MOVING and then came to rest.
+  # Distinct from leadLeft/leadRight, which are moving-target-only by design.
+  struct AdjacentStopped {
+    status @0 :Bool;
+    dRel @1 :Float32;
+    yRel @2 :Float32;
+    radarTrackId @3 :Int32 = -1;
+  }
 
   struct LeadData {
     dRel @0 :Float32;

common/params_keys.h

@@ -374,6 +374,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LaneCenteringE2EAuthority", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},
     {"LaneCenterOffset", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
     {"LaneChanges", {PERSISTENT, BOOL, "1", "1", 0, SETTINGS_SIMPLE}},
+    {"LaneChangeCloseGap", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
+    {"LaneChangeCloseGapSeconds", {PERSISTENT, FLOAT, "1.0", "1.0", 1, SETTINGS_SIMPLE}},
     {"LaneChangeSmoothing", {PERSISTENT, INT, "5", "10", 1, SETTINGS_SIMPLE}},
     {"LaneChangeTime", {PERSISTENT, FLOAT, "1.0", "0.0", 1, SETTINGS_SIMPLE}},
     {"LaneDetectionWidth", {PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}},

panda/board/obj/gitversion.h

@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-0351e7d8-DEBUG";
+const uint8_t gitversion[19] = "DEV-f6d38dbd-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-0351e7d8-DEBUG
+DEV-f6d38dbd-DEBUG

scripts/laptop_device_build.sh

@@ -166,7 +166,12 @@ expected_capnp_version() {
 
 image_capnp_version() {
   local engine="$1"
-  "${engine}" run --rm --platform linux/arm64 "${IMAGE_NAME}" bash -lc "capnp --version | awk '{print \$4}'" 2>/dev/null || true
+  local version_output=""
+
+  version_output="$("${engine}" run --rm --platform linux/arm64 "${IMAGE_NAME}" capnp --version 2>&1 || true)"
+  printf '%s\n' "${version_output}" \
+    | sed -nE 's/.*version[[:space:]]+([0-9]+\.[0-9]+\.[0-9]+).*/\1/p' \
+    | head -n 1
 }
 
 ensure_image_capnp_version() {

scripts/model_compiler.py

@@ -5,6 +5,7 @@ import hashlib
 import json
 import os
 import pickle
+import shutil
 import subprocess
 import sys
 from pathlib import Path
@@ -19,6 +20,7 @@ COMPILE_SCRIPT = REPO_ROOT / "tinygrad_repo/examples/openpilot/compile3.py"
 DRIVING_COMPILE_SCRIPT = REPO_ROOT / "selfdrive/modeld/compile_modeld.py"
 DM_WARP_COMPILE_SCRIPT = REPO_ROOT / "selfdrive/modeld/compile_dm_warp.py"
 MODEL_VERSIONS_CACHE = Path("/data/models/.model_versions.json")
+MODELS_PATH = MODEL_VERSIONS_CACHE.parent  # runtime dir modeld loads from: /data/models
 
 DM_MODEL_KEY = "dm"
 DM_MODEL_NAME = "dmonitoring_model"
@@ -84,6 +86,11 @@ def parse_args() -> argparse.Namespace:
                       help="Compile the driving artifact for the USB AMD GPU.")
   parser.add_argument("--split-artifact", type=Path, help="Split an existing oversized PKL without compiling.")
   parser.add_argument("--chunk-size-mib", type=int, default=95, help="Multipart size in MiB; must be below 100.")
+  parser.add_argument("--no-split", action="store_true",
+                      help="Keep a single .pkl even if >100 MiB (for local installs, which need one "
+                           "file). Auto-enabled for local- model IDs.")
+  parser.add_argument("--no-install", action="store_true",
+                      help="Do not auto-copy a local- model into /data/models after compiling.")
   parser.add_argument(
     "--image-history-pipeline",
     choices=("policy", "warp"),
@@ -302,6 +309,48 @@ def multipart_output_paths(artifact: Path, output_dir: Path | None = None) -> li
   ]
 
 
+def install_local_artifact(artifact: Path, model_key: str, version: str) -> None:
+  """Copy a freshly compiled local- model into the runtime dir modeld loads from,
+  and ensure its <id>.json sidecar carries the correct version.
+
+  The sidecar version is NOT cosmetic: without it _discover_local_models() records
+  an empty version, which downstream parses on the wrong contract (a v15 model then
+  drives like v11). Since we know the build version here, we write it so the local
+  install is correct by default. Local models must be a single is_file() in
+  /data/models to show in the picker. No-ops off-device (no /data/models).
+  """
+  if not MODELS_PATH.is_dir():
+    print(f"  skipped auto-install: {MODELS_PATH} not present (not on device?)")
+    return
+  dest = MODELS_PATH / artifact.name
+  shutil.copy2(artifact, dest)
+  print(f"  installed -> {dest}")
+
+  sidecar = MODELS_PATH / f"{model_key}.json"
+  info: dict = {}
+  if sidecar.is_file():
+    try:
+      loaded = json.loads(sidecar.read_text())
+      if isinstance(loaded, dict):
+        info = loaded
+    except Exception as error:
+      print(f"  WARN: existing sidecar {sidecar.name} is malformed, rewriting: {error}")
+  if not version:
+    if not str(info.get("version") or "").strip():
+      print(f"  WARN: could not determine version -- set it by hand in {sidecar.name} "
+            "or the model may drive on the wrong version contract")
+    return
+  # keep any user-set name/series; only guarantee a correct, non-empty version
+  if str(info.get("version") or "").strip() == version and sidecar.is_file():
+    print(f"  sidecar ok: {sidecar.name} (version {version})")
+    return
+  info.setdefault("name", model_key[len("local-"):].replace("_", " ").replace("-", " ").strip())
+  info.setdefault("series", "Local")
+  info["version"] = version
+  sidecar.write_text(json.dumps(info, indent=2) + "\n")
+  print(f"  wrote sidecar {sidecar.name} (version {version})")
+
+
 def split_oversized_artifact(
   artifact: Path,
   output_dir: Path | None = None,
@@ -507,17 +556,34 @@ def main() -> int:
   if not version and input_format == "supercombo":
     version = "v15"
   version_label = version or "unspecified behavior"
-  print(f"Compiling {model_key} ({input_format}, {version_label}) from {args.input_dir} -> {args.output_dir}")
+  will_install = model_key.startswith("local-") and not args.no_install
+  target = f"{args.output_dir}" + (f" -> {MODELS_PATH} (auto-install)" if will_install else "")
+  print(f"Compiling {model_key} ({input_format}, {version_label}) from {args.input_dir} -> {target}")
   output = compile_driving(model_key, files, input_format, version, args.output_dir,
                            args.image_history_pipeline, args.external_gpu)
   print(f"  saved {output.name}")
-  multipart_outputs = split_oversized_artifact(output)
-  if multipart_outputs:
-    print("  artifact exceeds 100 MiB; created repository-safe multipart files:")
-    for multipart_output in multipart_outputs:
-      print(f"    {multipart_output.name} ({multipart_output.stat().st_size} bytes)")
-    output.unlink()
-    print(f"  removed oversized source artifact {output.name}")
+  # Local models install as a single is_file() and never go to GitHub, so the >100 MiB
+  # repo split is pointless for them (you'd only have to reassemble it). Keep one .pkl.
+  keep_single = args.no_split or model_key.startswith("local-")
+  if keep_single:
+    is_local = model_key.startswith("local-")
+    if output.stat().st_size > REPOSITORY_FILE_LIMIT:
+      size_mb = output.stat().st_size / 1e6
+      if is_local:
+        print(f"  local model: kept as one {size_mb:.1f} MB file (repo split not needed)")
+      else:
+        print(f"  --no-split: kept one {size_mb:.1f} MB file; over 100 MB, so split it "
+              "before committing to a repo (re-run without --no-split, or --split-artifact)")
+    if is_local and not args.no_install:
+      install_local_artifact(output, model_key, version)
+  else:
+    multipart_outputs = split_oversized_artifact(output)
+    if multipart_outputs:
+      print("  artifact exceeds 100 MiB; created repository-safe multipart files:")
+      for multipart_output in multipart_outputs:
+        print(f"    {multipart_output.name} ({multipart_output.stat().st_size} bytes)")
+      output.unlink()
+      print(f"  removed oversized source artifact {output.name}")
   print("Done.")
   return 0
 

selfdrive/controls/lib/latcontrol_torque.py

@@ -388,7 +388,7 @@ class LatControlTorque(LatControl):
         friction_scale *= get_trailer_lateral_friction_scale(trailer_load_kg, CS.vEgo, setpoint)
       friction_jerk = desired_lateral_jerk
       if ioniq_6_active:
-        # planner jerk noise on straights (< ~0.3 m/s^3) chatters the friction compensation
+        # planner jerk noise on straights (p95 ~0.5 m/s^3) chatters the friction compensation
         friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - IONIQ_6_FRICTION_JERK_DEADZONE, 0.0), desired_lateral_jerk)
       ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * friction_jerk, lateral_accel_deadzone, friction_threshold, self.torque_params)
       deadzone_boost_active = False

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -653,7 +653,17 @@ IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS
 # (~0.5 Hz) weave on straights: the 0.09/0.39 small-signal slope plus the jerk feed acts as
 # extra P/D gain right where there is no breakaway torque to overcome. Deadzone the jerk
 # feed below straight-line noise levels and fade friction near center at highway speed.
-IONIQ_6_FRICTION_JERK_DEADZONE = 0.30
+# 2026-07-23: raised 0.30 -> 0.50. The 0.30 value was set when straight-line |desiredLatJerk|
+# p95 was 0.286; on the "straights" logs (70 mph, hands off) it is now p95 0.48-0.58 with
+# 17-35% of frames past the deadzone, so the jerk feed supplies 43-78% of the feedforward's
+# energy at the 0.66-0.74 Hz weave. That weave is a delay-driven loop resonance, so the fix
+# has to remove GAIN at the tone, not add phase lag. 0.50 cuts the jerk path's tone-band
+# content 44-82% while a real 2.0 m/s^3 turn-in still keeps 88% of its friction comp.
+# Bounded elsewhere: the friction argument moves by at most JERK_GAIN*0.20 = 0.044 m/s^2
+# (<=11.3% of friction authority vs the 0.39 threshold floor) and is exactly zero wherever
+# get_friction is already saturated -- 93% of sub-25 mph turn frames, so slow/tight turns
+# and the stop-turn pre-wind are untouched. Mean change is <0.25% below 45 mph.
+IONIQ_6_FRICTION_JERK_DEADZONE = 0.50
 IONIQ_6_FRICTION_CENTER_FADE_MAX = 0.50
 IONIQ_6_FRICTION_CENTER_FADE_LAT = 0.15
 IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH = 0.06

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -835,7 +835,8 @@ class LongitudinalMpc:
 
   def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow,
              personality=log.LongitudinalPersonality.standard, tracking_lead=True,
-             optional_far_lead_comfort=True, smooth_duplicate_vision=False):
+             optional_far_lead_comfort=True, smooth_duplicate_vision=False,
+             stop_x=None):
     v_ego = self.x0[1]
     lead_one = radarstate.leadOne
     lead_two = radarstate.leadTwo
@@ -890,6 +891,11 @@ class LongitudinalMpc:
         lead_0_bias, lead_1_bias = self.get_near_duplicate_lead_source_hysteresis(prev_source, lead_one, lead_two, v_ego)
         lead_0_obstacle = lead_0_obstacle + lead_0_bias
         lead_1_obstacle = lead_1_obstacle + lead_1_bias
+      # A forced stop is a position constraint, not a speed one. Folding it into the cruise
+      # obstacle lets the solver plan the stop directly instead of chasing a descending
+      # speed ceiling, which it tracks with a persistent lag.
+      if stop_x is not None:
+        cruise_obstacle = np.minimum(cruise_obstacle, stop_x)
       x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
       candidate_source = SOURCES[np.argmin(x_obstacles[0])]
       sticky_source = None
@@ -939,6 +945,8 @@ class LongitudinalMpc:
       xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
       x = np.cumsum(np.insert(xforward, 0, x[0]))
 
+      if stop_x is not None:
+        cruise_target = np.minimum(cruise_target, stop_x)
       x_and_cruise = np.column_stack([x, cruise_target])
       x = np.min(x_and_cruise, axis=1)
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -3095,11 +3095,21 @@ class LongitudinalPlanner:
     dec_mpc_mode = self.get_mpc_mode()
     if not self.mlsim:
       self.mpc.mode = dec_mpc_mode
+    # Hand the forced stop to the solver as a position. forcingStopLength is the tracked
+    # distance to the line; the obstacle sits STOP_DISTANCE beyond it so the safe-distance
+    # term brings the car to rest on the line rather than short of it. Below the handoff
+    # distance forcingStopLength is already decaying to zero, so leave those last few metres
+    # to the existing v_cruise=0 path instead of placing an obstacle behind the car.
+    force_stop_x = None
+    if sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength > STOP_DISTANCE:
+      force_stop_x = float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE
+
     self.mpc.update(sm['radarState'], v_cruise, x, v, a, j,
                     sm['starpilotPlan'].dangerFactor, effective_t_follow,
                     personality=personality, tracking_lead=lead_control_active,
                     optional_far_lead_comfort=True,
-                    smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass)
+                    smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass,
+                    stop_x=force_stop_x)
 
     self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)

selfdrive/controls/radard.py

@@ -30,6 +30,22 @@ RADAR_TO_CAMERA = 1.52  # RADAR is ~ 1.5m ahead from center of mesh frame
 G90_RADAR_LOW_SPEED_MAX_DIST = 12.0
 G90_RADAR_LOW_SPEED_MAX_Y = 0.6
 
+# Adjacent-lane stopped-vehicle detector, used as a stop-line hint on red-light
+# approaches. The qualifier is the DECELERATION HISTORY, not the current speed: a target
+# that was seen moving and then came to rest is self-identifying, while roadside
+# furniture and curb-parked cars never show that transition. Simply accepting "anything
+# slow in the next lane" would pick up parked cars and brake us early.
+# Measured over 39 red-light approaches (Desktop/stops corpus): present on ~10% of stops,
+# dRel reads a mean 6.3 m SHORT of the true stop line (worst case 13.7 m short, a car
+# queued behind the bar), and first appears a median 53 m out.
+ADJACENT_STOP_MOVING_V = 5.0      # m/s — must have genuinely been moving
+ADJACENT_STOP_REST_V = 1.5        # m/s — and then genuinely at rest
+ADJACENT_STOP_MOVING_FRAMES = 15  # 0.75 s at 20 Hz, both ways: rejects speed noise
+ADJACENT_STOP_REST_FRAMES = 15
+ADJACENT_STOP_MIN_Y = 1.8         # m — inside this is our own lane
+ADJACENT_STOP_MAX_Y = 7.5         # m — beyond this is roadside, not an adjacent lane
+ADJACENT_STOP_MAX_D = 110.0       # m
+
 
 class KalmanParams:
   def __init__(self, dt: float):
@@ -62,6 +78,11 @@ class Track:
 
     self.leadTrackID = 0
 
+    # deceleration history for the adjacent-lane stopped-vehicle detector
+    self.moving_frames = 0
+    self.rest_frames = 0
+    self.seen_moving = False
+
   def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
     # relative values, copy
     self.dRel = d_rel   # LONG_DIST
@@ -83,6 +104,21 @@ class Track:
     else:
       self.aLeadTau.update(0.0)
 
+    # Track the moving -> stopped transition. Only a sustained run counts, so a single
+    # noisy speed sample can neither arm nor trip the detector.
+    if self.vLead > ADJACENT_STOP_MOVING_V:
+      self.moving_frames += 1
+      self.rest_frames = 0
+      if self.moving_frames >= ADJACENT_STOP_MOVING_FRAMES:
+        self.seen_moving = True
+    elif abs(self.vLead) < ADJACENT_STOP_REST_V:
+      self.moving_frames = 0
+      self.rest_frames += 1
+    else:
+      # coasting between the two bands: hold state, restart both runs
+      self.moving_frames = 0
+      self.rest_frames = 0
+
     self.cnt += 1
 
   def get_RadarState(self, model_prob: float = 0.0):
@@ -111,6 +147,33 @@ class Track:
     right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
     return -self.yRel > right_lane
 
+  def is_adjacent_stopped(self, model_data: capnp._DynamicStructReader):
+    """A neighbouring-lane vehicle that was seen moving and has now come to rest.
+
+    Deliberately NOT the same test as potential_adjacent_lead, which is moving-target-only
+    and would have to be loosened to "anything slow" to catch these — that would also
+    catch curb-parked cars. Requiring the moving -> stopped transition keeps the signal
+    self-validating. Lane geometry mirrors potential_adjacent_lead (model y == -yRel,
+    laneLines[1] the left boundary and laneLines[2] the right), plus an outer bound so
+    roadside returns past the neighbouring lane don't qualify.
+    """
+    if not (self.seen_moving and self.rest_frames >= ADJACENT_STOP_REST_FRAMES):
+      return False
+
+    if self.leadTrackID == self.identifier:
+      return False
+
+    if not (ADJACENT_STOP_MIN_Y < abs(self.yRel) < ADJACENT_STOP_MAX_Y):
+      return False
+
+    if not (0.0 < self.dRel < ADJACENT_STOP_MAX_D):
+      return False
+
+    model_y = -self.yRel
+    left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y)
+    right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
+    return bool(model_y < left_lane or model_y > right_lane)
+
   def potential_low_speed_lead(self, v_ego: float):
     # stop for stuff in front of you and low speed, even without model confirmation
     # Radar points closer than 0.75, are almost always glitches on toyota radars
@@ -270,6 +333,31 @@ def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: ca
   return lead_dict
 
 
+def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStructReader) -> dict[str, Any]:
+  """Stop-line hint: a vehicle that decelerated to a stop in a neighbouring lane.
+
+  Takes the FARTHEST qualifying vehicle, not the nearest. In a queue the front car sits
+  at the stop bar and the ones behind it are progressively closer to us, so the nearest
+  candidate systematically underestimates the distance to the line. Since the consumer
+  only ever uses this to SHORTEN its estimate, underestimating is the harmful direction —
+  picking the farthest degrades toward doing nothing instead.
+  """
+  if len(model_data.laneLines) < 4:
+    return {'status': False}
+
+  candidates = [c for c in tracks.values() if c.is_adjacent_stopped(model_data)]
+  if not candidates:
+    return {'status': False}
+
+  furthest = max(candidates, key=lambda c: c.dRel)
+  return {
+    'status': True,
+    'dRel': float(furthest.dRel),
+    'yRel': float(furthest.yRel),
+    'radarTrackId': int(furthest.identifier),
+  }
+
+
 class RadarD:
   def __init__(self, radar_ts: float = DT_MDL, delay: float = 0.0, g90_radar_filter: bool = False):
     self.current_time = 0.0
@@ -360,6 +448,12 @@ class RadarD:
       self.starpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True)
       self.starpilot_radar_state.leadRight = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=False)
 
+    # Not gated on the adjacent-lead toggles: this is a separate signal with a separate
+    # consumer (Force Stop), and leaving leadLeft/leadRight untouched keeps the existing
+    # lane-change and UI behaviour exactly as it was.
+    if self.ready:
+      self.starpilot_radar_state.adjacentStopped = get_adjacent_stopped(self.tracks, sm['modelV2'])
+
     self.starpilot_toggles = get_starpilot_toggles(sm)
 
   def publish(self, pm: messaging.PubMaster):

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_3329877133651101623) {
-   out_3329877133651101623[0] = delta_x[0] + nom_x[0];
-   out_3329877133651101623[1] = delta_x[1] + nom_x[1];
-   out_3329877133651101623[2] = delta_x[2] + nom_x[2];
-   out_3329877133651101623[3] = delta_x[3] + nom_x[3];
-   out_3329877133651101623[4] = delta_x[4] + nom_x[4];
-   out_3329877133651101623[5] = delta_x[5] + nom_x[5];
-   out_3329877133651101623[6] = delta_x[6] + nom_x[6];
-   out_3329877133651101623[7] = delta_x[7] + nom_x[7];
-   out_3329877133651101623[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_5959951015159528314) {
+   out_5959951015159528314[0] = delta_x[0] + nom_x[0];
+   out_5959951015159528314[1] = delta_x[1] + nom_x[1];
+   out_5959951015159528314[2] = delta_x[2] + nom_x[2];
+   out_5959951015159528314[3] = delta_x[3] + nom_x[3];
+   out_5959951015159528314[4] = delta_x[4] + nom_x[4];
+   out_5959951015159528314[5] = delta_x[5] + nom_x[5];
+   out_5959951015159528314[6] = delta_x[6] + nom_x[6];
+   out_5959951015159528314[7] = delta_x[7] + nom_x[7];
+   out_5959951015159528314[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_7760882190653564987) {
-   out_7760882190653564987[0] = -nom_x[0] + true_x[0];
-   out_7760882190653564987[1] = -nom_x[1] + true_x[1];
-   out_7760882190653564987[2] = -nom_x[2] + true_x[2];
-   out_7760882190653564987[3] = -nom_x[3] + true_x[3];
-   out_7760882190653564987[4] = -nom_x[4] + true_x[4];
-   out_7760882190653564987[5] = -nom_x[5] + true_x[5];
-   out_7760882190653564987[6] = -nom_x[6] + true_x[6];
-   out_7760882190653564987[7] = -nom_x[7] + true_x[7];
-   out_7760882190653564987[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_5356992719595228440) {
+   out_5356992719595228440[0] = -nom_x[0] + true_x[0];
+   out_5356992719595228440[1] = -nom_x[1] + true_x[1];
+   out_5356992719595228440[2] = -nom_x[2] + true_x[2];
+   out_5356992719595228440[3] = -nom_x[3] + true_x[3];
+   out_5356992719595228440[4] = -nom_x[4] + true_x[4];
+   out_5356992719595228440[5] = -nom_x[5] + true_x[5];
+   out_5356992719595228440[6] = -nom_x[6] + true_x[6];
+   out_5356992719595228440[7] = -nom_x[7] + true_x[7];
+   out_5356992719595228440[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_1773647306661711299) {
-   out_1773647306661711299[0] = 1.0;
-   out_1773647306661711299[1] = 0.0;
-   out_1773647306661711299[2] = 0.0;
-   out_1773647306661711299[3] = 0.0;
-   out_1773647306661711299[4] = 0.0;
-   out_1773647306661711299[5] = 0.0;
-   out_1773647306661711299[6] = 0.0;
-   out_1773647306661711299[7] = 0.0;
-   out_1773647306661711299[8] = 0.0;
-   out_1773647306661711299[9] = 0.0;
-   out_1773647306661711299[10] = 1.0;
-   out_1773647306661711299[11] = 0.0;
-   out_1773647306661711299[12] = 0.0;
-   out_1773647306661711299[13] = 0.0;
-   out_1773647306661711299[14] = 0.0;
-   out_1773647306661711299[15] = 0.0;
-   out_1773647306661711299[16] = 0.0;
-   out_1773647306661711299[17] = 0.0;
-   out_1773647306661711299[18] = 0.0;
-   out_1773647306661711299[19] = 0.0;
-   out_1773647306661711299[20] = 1.0;
-   out_1773647306661711299[21] = 0.0;
-   out_1773647306661711299[22] = 0.0;
-   out_1773647306661711299[23] = 0.0;
-   out_1773647306661711299[24] = 0.0;
-   out_1773647306661711299[25] = 0.0;
-   out_1773647306661711299[26] = 0.0;
-   out_1773647306661711299[27] = 0.0;
-   out_1773647306661711299[28] = 0.0;
-   out_1773647306661711299[29] = 0.0;
-   out_1773647306661711299[30] = 1.0;
-   out_1773647306661711299[31] = 0.0;
-   out_1773647306661711299[32] = 0.0;
-   out_1773647306661711299[33] = 0.0;
-   out_1773647306661711299[34] = 0.0;
-   out_1773647306661711299[35] = 0.0;
-   out_1773647306661711299[36] = 0.0;
-   out_1773647306661711299[37] = 0.0;
-   out_1773647306661711299[38] = 0.0;
-   out_1773647306661711299[39] = 0.0;
-   out_1773647306661711299[40] = 1.0;
-   out_1773647306661711299[41] = 0.0;
-   out_1773647306661711299[42] = 0.0;
-   out_1773647306661711299[43] = 0.0;
-   out_1773647306661711299[44] = 0.0;
-   out_1773647306661711299[45] = 0.0;
-   out_1773647306661711299[46] = 0.0;
-   out_1773647306661711299[47] = 0.0;
-   out_1773647306661711299[48] = 0.0;
-   out_1773647306661711299[49] = 0.0;
-   out_1773647306661711299[50] = 1.0;
-   out_1773647306661711299[51] = 0.0;
-   out_1773647306661711299[52] = 0.0;
-   out_1773647306661711299[53] = 0.0;
-   out_1773647306661711299[54] = 0.0;
-   out_1773647306661711299[55] = 0.0;
-   out_1773647306661711299[56] = 0.0;
-   out_1773647306661711299[57] = 0.0;
-   out_1773647306661711299[58] = 0.0;
-   out_1773647306661711299[59] = 0.0;
-   out_1773647306661711299[60] = 1.0;
-   out_1773647306661711299[61] = 0.0;
-   out_1773647306661711299[62] = 0.0;
-   out_1773647306661711299[63] = 0.0;
-   out_1773647306661711299[64] = 0.0;
-   out_1773647306661711299[65] = 0.0;
-   out_1773647306661711299[66] = 0.0;
-   out_1773647306661711299[67] = 0.0;
-   out_1773647306661711299[68] = 0.0;
-   out_1773647306661711299[69] = 0.0;
-   out_1773647306661711299[70] = 1.0;
-   out_1773647306661711299[71] = 0.0;
-   out_1773647306661711299[72] = 0.0;
-   out_1773647306661711299[73] = 0.0;
-   out_1773647306661711299[74] = 0.0;
-   out_1773647306661711299[75] = 0.0;
-   out_1773647306661711299[76] = 0.0;
-   out_1773647306661711299[77] = 0.0;
-   out_1773647306661711299[78] = 0.0;
-   out_1773647306661711299[79] = 0.0;
-   out_1773647306661711299[80] = 1.0;
+void H_mod_fun(double *state, double *out_81214266615305410) {
+   out_81214266615305410[0] = 1.0;
+   out_81214266615305410[1] = 0.0;
+   out_81214266615305410[2] = 0.0;
+   out_81214266615305410[3] = 0.0;
+   out_81214266615305410[4] = 0.0;
+   out_81214266615305410[5] = 0.0;
+   out_81214266615305410[6] = 0.0;
+   out_81214266615305410[7] = 0.0;
+   out_81214266615305410[8] = 0.0;
+   out_81214266615305410[9] = 0.0;
+   out_81214266615305410[10] = 1.0;
+   out_81214266615305410[11] = 0.0;
+   out_81214266615305410[12] = 0.0;
+   out_81214266615305410[13] = 0.0;
+   out_81214266615305410[14] = 0.0;
+   out_81214266615305410[15] = 0.0;
+   out_81214266615305410[16] = 0.0;
+   out_81214266615305410[17] = 0.0;
+   out_81214266615305410[18] = 0.0;
+   out_81214266615305410[19] = 0.0;
+   out_81214266615305410[20] = 1.0;
+   out_81214266615305410[21] = 0.0;
+   out_81214266615305410[22] = 0.0;
+   out_81214266615305410[23] = 0.0;
+   out_81214266615305410[24] = 0.0;
+   out_81214266615305410[25] = 0.0;
+   out_81214266615305410[26] = 0.0;
+   out_81214266615305410[27] = 0.0;
+   out_81214266615305410[28] = 0.0;
+   out_81214266615305410[29] = 0.0;
+   out_81214266615305410[30] = 1.0;
+   out_81214266615305410[31] = 0.0;
+   out_81214266615305410[32] = 0.0;
+   out_81214266615305410[33] = 0.0;
+   out_81214266615305410[34] = 0.0;
+   out_81214266615305410[35] = 0.0;
+   out_81214266615305410[36] = 0.0;
+   out_81214266615305410[37] = 0.0;
+   out_81214266615305410[38] = 0.0;
+   out_81214266615305410[39] = 0.0;
+   out_81214266615305410[40] = 1.0;
+   out_81214266615305410[41] = 0.0;
+   out_81214266615305410[42] = 0.0;
+   out_81214266615305410[43] = 0.0;
+   out_81214266615305410[44] = 0.0;
+   out_81214266615305410[45] = 0.0;
+   out_81214266615305410[46] = 0.0;
+   out_81214266615305410[47] = 0.0;
+   out_81214266615305410[48] = 0.0;
+   out_81214266615305410[49] = 0.0;
+   out_81214266615305410[50] = 1.0;
+   out_81214266615305410[51] = 0.0;
+   out_81214266615305410[52] = 0.0;
+   out_81214266615305410[53] = 0.0;
+   out_81214266615305410[54] = 0.0;
+   out_81214266615305410[55] = 0.0;
+   out_81214266615305410[56] = 0.0;
+   out_81214266615305410[57] = 0.0;
+   out_81214266615305410[58] = 0.0;
+   out_81214266615305410[59] = 0.0;
+   out_81214266615305410[60] = 1.0;
+   out_81214266615305410[61] = 0.0;
+   out_81214266615305410[62] = 0.0;
+   out_81214266615305410[63] = 0.0;
+   out_81214266615305410[64] = 0.0;
+   out_81214266615305410[65] = 0.0;
+   out_81214266615305410[66] = 0.0;
+   out_81214266615305410[67] = 0.0;
+   out_81214266615305410[68] = 0.0;
+   out_81214266615305410[69] = 0.0;
+   out_81214266615305410[70] = 1.0;
+   out_81214266615305410[71] = 0.0;
+   out_81214266615305410[72] = 0.0;
+   out_81214266615305410[73] = 0.0;
+   out_81214266615305410[74] = 0.0;
+   out_81214266615305410[75] = 0.0;
+   out_81214266615305410[76] = 0.0;
+   out_81214266615305410[77] = 0.0;
+   out_81214266615305410[78] = 0.0;
+   out_81214266615305410[79] = 0.0;
+   out_81214266615305410[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_8265395554005205627) {
-   out_8265395554005205627[0] = state[0];
-   out_8265395554005205627[1] = state[1];
-   out_8265395554005205627[2] = state[2];
-   out_8265395554005205627[3] = state[3];
-   out_8265395554005205627[4] = state[4];
-   out_8265395554005205627[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_8265395554005205627[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_8265395554005205627[7] = state[7];
-   out_8265395554005205627[8] = state[8];
+void f_fun(double *state, double dt, double *out_3378346814935937794) {
+   out_3378346814935937794[0] = state[0];
+   out_3378346814935937794[1] = state[1];
+   out_3378346814935937794[2] = state[2];
+   out_3378346814935937794[3] = state[3];
+   out_3378346814935937794[4] = state[4];
+   out_3378346814935937794[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_3378346814935937794[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_3378346814935937794[7] = state[7];
+   out_3378346814935937794[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_5121501708015469294) {
-   out_5121501708015469294[0] = 1;
-   out_5121501708015469294[1] = 0;
-   out_5121501708015469294[2] = 0;
-   out_5121501708015469294[3] = 0;
-   out_5121501708015469294[4] = 0;
-   out_5121501708015469294[5] = 0;
-   out_5121501708015469294[6] = 0;
-   out_5121501708015469294[7] = 0;
-   out_5121501708015469294[8] = 0;
-   out_5121501708015469294[9] = 0;
-   out_5121501708015469294[10] = 1;
-   out_5121501708015469294[11] = 0;
-   out_5121501708015469294[12] = 0;
-   out_5121501708015469294[13] = 0;
-   out_5121501708015469294[14] = 0;
-   out_5121501708015469294[15] = 0;
-   out_5121501708015469294[16] = 0;
-   out_5121501708015469294[17] = 0;
-   out_5121501708015469294[18] = 0;
-   out_5121501708015469294[19] = 0;
-   out_5121501708015469294[20] = 1;
-   out_5121501708015469294[21] = 0;
-   out_5121501708015469294[22] = 0;
-   out_5121501708015469294[23] = 0;
-   out_5121501708015469294[24] = 0;
-   out_5121501708015469294[25] = 0;
-   out_5121501708015469294[26] = 0;
-   out_5121501708015469294[27] = 0;
-   out_5121501708015469294[28] = 0;
-   out_5121501708015469294[29] = 0;
-   out_5121501708015469294[30] = 1;
-   out_5121501708015469294[31] = 0;
-   out_5121501708015469294[32] = 0;
-   out_5121501708015469294[33] = 0;
-   out_5121501708015469294[34] = 0;
-   out_5121501708015469294[35] = 0;
-   out_5121501708015469294[36] = 0;
-   out_5121501708015469294[37] = 0;
-   out_5121501708015469294[38] = 0;
-   out_5121501708015469294[39] = 0;
-   out_5121501708015469294[40] = 1;
-   out_5121501708015469294[41] = 0;
-   out_5121501708015469294[42] = 0;
-   out_5121501708015469294[43] = 0;
-   out_5121501708015469294[44] = 0;
-   out_5121501708015469294[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_5121501708015469294[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_5121501708015469294[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_5121501708015469294[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_5121501708015469294[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_5121501708015469294[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_5121501708015469294[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_5121501708015469294[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_5121501708015469294[53] = -9.8100000000000005*dt;
-   out_5121501708015469294[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_5121501708015469294[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_5121501708015469294[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_5121501708015469294[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_5121501708015469294[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_5121501708015469294[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_5121501708015469294[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_5121501708015469294[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_5121501708015469294[62] = 0;
-   out_5121501708015469294[63] = 0;
-   out_5121501708015469294[64] = 0;
-   out_5121501708015469294[65] = 0;
-   out_5121501708015469294[66] = 0;
-   out_5121501708015469294[67] = 0;
-   out_5121501708015469294[68] = 0;
-   out_5121501708015469294[69] = 0;
-   out_5121501708015469294[70] = 1;
-   out_5121501708015469294[71] = 0;
-   out_5121501708015469294[72] = 0;
-   out_5121501708015469294[73] = 0;
-   out_5121501708015469294[74] = 0;
-   out_5121501708015469294[75] = 0;
-   out_5121501708015469294[76] = 0;
-   out_5121501708015469294[77] = 0;
-   out_5121501708015469294[78] = 0;
-   out_5121501708015469294[79] = 0;
-   out_5121501708015469294[80] = 1;
+void F_fun(double *state, double dt, double *out_4090498247982689107) {
+   out_4090498247982689107[0] = 1;
+   out_4090498247982689107[1] = 0;
+   out_4090498247982689107[2] = 0;
+   out_4090498247982689107[3] = 0;
+   out_4090498247982689107[4] = 0;
+   out_4090498247982689107[5] = 0;
+   out_4090498247982689107[6] = 0;
+   out_4090498247982689107[7] = 0;
+   out_4090498247982689107[8] = 0;
+   out_4090498247982689107[9] = 0;
+   out_4090498247982689107[10] = 1;
+   out_4090498247982689107[11] = 0;
+   out_4090498247982689107[12] = 0;
+   out_4090498247982689107[13] = 0;
+   out_4090498247982689107[14] = 0;
+   out_4090498247982689107[15] = 0;
+   out_4090498247982689107[16] = 0;
+   out_4090498247982689107[17] = 0;
+   out_4090498247982689107[18] = 0;
+   out_4090498247982689107[19] = 0;
+   out_4090498247982689107[20] = 1;
+   out_4090498247982689107[21] = 0;
+   out_4090498247982689107[22] = 0;
+   out_4090498247982689107[23] = 0;
+   out_4090498247982689107[24] = 0;
+   out_4090498247982689107[25] = 0;
+   out_4090498247982689107[26] = 0;
+   out_4090498247982689107[27] = 0;
+   out_4090498247982689107[28] = 0;
+   out_4090498247982689107[29] = 0;
+   out_4090498247982689107[30] = 1;
+   out_4090498247982689107[31] = 0;
+   out_4090498247982689107[32] = 0;
+   out_4090498247982689107[33] = 0;
+   out_4090498247982689107[34] = 0;
+   out_4090498247982689107[35] = 0;
+   out_4090498247982689107[36] = 0;
+   out_4090498247982689107[37] = 0;
+   out_4090498247982689107[38] = 0;
+   out_4090498247982689107[39] = 0;
+   out_4090498247982689107[40] = 1;
+   out_4090498247982689107[41] = 0;
+   out_4090498247982689107[42] = 0;
+   out_4090498247982689107[43] = 0;
+   out_4090498247982689107[44] = 0;
+   out_4090498247982689107[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_4090498247982689107[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_4090498247982689107[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_4090498247982689107[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_4090498247982689107[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_4090498247982689107[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_4090498247982689107[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_4090498247982689107[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_4090498247982689107[53] = -9.8100000000000005*dt;
+   out_4090498247982689107[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_4090498247982689107[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_4090498247982689107[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_4090498247982689107[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_4090498247982689107[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_4090498247982689107[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_4090498247982689107[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_4090498247982689107[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_4090498247982689107[62] = 0;
+   out_4090498247982689107[63] = 0;
+   out_4090498247982689107[64] = 0;
+   out_4090498247982689107[65] = 0;
+   out_4090498247982689107[66] = 0;
+   out_4090498247982689107[67] = 0;
+   out_4090498247982689107[68] = 0;
+   out_4090498247982689107[69] = 0;
+   out_4090498247982689107[70] = 1;
+   out_4090498247982689107[71] = 0;
+   out_4090498247982689107[72] = 0;
+   out_4090498247982689107[73] = 0;
+   out_4090498247982689107[74] = 0;
+   out_4090498247982689107[75] = 0;
+   out_4090498247982689107[76] = 0;
+   out_4090498247982689107[77] = 0;
+   out_4090498247982689107[78] = 0;
+   out_4090498247982689107[79] = 0;
+   out_4090498247982689107[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_1871114119926472838) {
-   out_1871114119926472838[0] = state[6];
+void h_25(double *state, double *unused, double *out_2941759932276218077) {
+   out_2941759932276218077[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_1749401331983653849) {
-   out_1749401331983653849[0] = 0;
-   out_1749401331983653849[1] = 0;
-   out_1749401331983653849[2] = 0;
-   out_1749401331983653849[3] = 0;
-   out_1749401331983653849[4] = 0;
-   out_1749401331983653849[5] = 0;
-   out_1749401331983653849[6] = 1;
-   out_1749401331983653849[7] = 0;
-   out_1749401331983653849[8] = 0;
+void H_25(double *state, double *unused, double *out_8002552299589165215) {
+   out_8002552299589165215[0] = 0;
+   out_8002552299589165215[1] = 0;
+   out_8002552299589165215[2] = 0;
+   out_8002552299589165215[3] = 0;
+   out_8002552299589165215[4] = 0;
+   out_8002552299589165215[5] = 0;
+   out_8002552299589165215[6] = 1;
+   out_8002552299589165215[7] = 0;
+   out_8002552299589165215[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_4177328874243606255) {
-   out_4177328874243606255[0] = state[4];
-   out_4177328874243606255[1] = state[5];
+void h_24(double *state, double *unused, double *out_3100385683736896980) {
+   out_3100385683736896980[0] = state[4];
+   out_3100385683736896980[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_1808524266818669287) {
-   out_1808524266818669287[0] = 0;
-   out_1808524266818669287[1] = 0;
-   out_1808524266818669287[2] = 0;
-   out_1808524266818669287[3] = 0;
-   out_1808524266818669287[4] = 1;
-   out_1808524266818669287[5] = 0;
-   out_1808524266818669287[6] = 0;
-   out_1808524266818669287[7] = 0;
-   out_1808524266818669287[8] = 0;
-   out_1808524266818669287[9] = 0;
-   out_1808524266818669287[10] = 0;
-   out_1808524266818669287[11] = 0;
-   out_1808524266818669287[12] = 0;
-   out_1808524266818669287[13] = 0;
-   out_1808524266818669287[14] = 1;
-   out_1808524266818669287[15] = 0;
-   out_1808524266818669287[16] = 0;
-   out_1808524266818669287[17] = 0;
+void H_24(double *state, double *unused, double *out_8986618170430478883) {
+   out_8986618170430478883[0] = 0;
+   out_8986618170430478883[1] = 0;
+   out_8986618170430478883[2] = 0;
+   out_8986618170430478883[3] = 0;
+   out_8986618170430478883[4] = 1;
+   out_8986618170430478883[5] = 0;
+   out_8986618170430478883[6] = 0;
+   out_8986618170430478883[7] = 0;
+   out_8986618170430478883[8] = 0;
+   out_8986618170430478883[9] = 0;
+   out_8986618170430478883[10] = 0;
+   out_8986618170430478883[11] = 0;
+   out_8986618170430478883[12] = 0;
+   out_8986618170430478883[13] = 0;
+   out_8986618170430478883[14] = 1;
+   out_8986618170430478883[15] = 0;
+   out_8986618170430478883[16] = 0;
+   out_8986618170430478883[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_6625546561337267154) {
-   out_6625546561337267154[0] = state[4];
+void h_30(double *state, double *unused, double *out_4392391324905101244) {
+   out_4392391324905101244[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_5167289009507962906) {
-   out_5167289009507962906[0] = 0;
-   out_5167289009507962906[1] = 0;
-   out_5167289009507962906[2] = 0;
-   out_5167289009507962906[3] = 0;
-   out_5167289009507962906[4] = 1;
-   out_5167289009507962906[5] = 0;
-   out_5167289009507962906[6] = 0;
-   out_5167289009507962906[7] = 0;
-   out_5167289009507962906[8] = 0;
+void H_30(double *state, double *unused, double *out_3474855969461557017) {
+   out_3474855969461557017[0] = 0;
+   out_3474855969461557017[1] = 0;
+   out_3474855969461557017[2] = 0;
+   out_3474855969461557017[3] = 0;
+   out_3474855969461557017[4] = 1;
+   out_3474855969461557017[5] = 0;
+   out_3474855969461557017[6] = 0;
+   out_3474855969461557017[7] = 0;
+   out_3474855969461557017[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_8545349699624827873) {
-   out_8545349699624827873[0] = state[7];
+void h_26(double *state, double *unused, double *out_3042752109523599569) {
+   out_3042752109523599569[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_5490904650857710073) {
-   out_5490904650857710073[0] = 0;
-   out_5490904650857710073[1] = 0;
-   out_5490904650857710073[2] = 0;
-   out_5490904650857710073[3] = 0;
-   out_5490904650857710073[4] = 0;
-   out_5490904650857710073[5] = 0;
-   out_5490904650857710073[6] = 0;
-   out_5490904650857710073[7] = 1;
-   out_5490904650857710073[8] = 0;
+void H_26(double *state, double *unused, double *out_4261048980715108991) {
+   out_4261048980715108991[0] = 0;
+   out_4261048980715108991[1] = 0;
+   out_4261048980715108991[2] = 0;
+   out_4261048980715108991[3] = 0;
+   out_4261048980715108991[4] = 0;
+   out_4261048980715108991[5] = 0;
+   out_4261048980715108991[6] = 0;
+   out_4261048980715108991[7] = 1;
+   out_4261048980715108991[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_3470753115959557725) {
-   out_3470753115959557725[0] = state[3];
+void h_27(double *state, double *unused, double *out_6523014969426492774) {
+   out_6523014969426492774[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_2992525697707537995) {
-   out_2992525697707537995[0] = 0;
-   out_2992525697707537995[1] = 0;
-   out_2992525697707537995[2] = 0;
-   out_2992525697707537995[3] = 1;
-   out_2992525697707537995[4] = 0;
-   out_2992525697707537995[5] = 0;
-   out_2992525697707537995[6] = 0;
-   out_2992525697707537995[7] = 0;
-   out_2992525697707537995[8] = 0;
+void H_27(double *state, double *unused, double *out_5698450040645500234) {
+   out_5698450040645500234[0] = 0;
+   out_5698450040645500234[1] = 0;
+   out_5698450040645500234[2] = 0;
+   out_5698450040645500234[3] = 1;
+   out_5698450040645500234[4] = 0;
+   out_5698450040645500234[5] = 0;
+   out_5698450040645500234[6] = 0;
+   out_5698450040645500234[7] = 0;
+   out_5698450040645500234[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_7569056414238929589) {
-   out_7569056414238929589[0] = state[1];
+void h_29(double *state, double *unused, double *out_609281942755744401) {
+   out_609281942755744401[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_1279162970837986962) {
-   out_1279162970837986962[0] = 0;
-   out_1279162970837986962[1] = 1;
-   out_1279162970837986962[2] = 0;
-   out_1279162970837986962[3] = 0;
-   out_1279162970837986962[4] = 0;
-   out_1279162970837986962[5] = 0;
-   out_1279162970837986962[6] = 0;
-   out_1279162970837986962[7] = 0;
-   out_1279162970837986962[8] = 0;
+void H_29(double *state, double *unused, double *out_3985087313775949201) {
+   out_3985087313775949201[0] = 0;
+   out_3985087313775949201[1] = 1;
+   out_3985087313775949201[2] = 0;
+   out_3985087313775949201[3] = 0;
+   out_3985087313775949201[4] = 0;
+   out_3985087313775949201[5] = 0;
+   out_3985087313775949201[6] = 0;
+   out_3985087313775949201[7] = 0;
+   out_3985087313775949201[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_6507539167403890410) {
-   out_6507539167403890410[0] = state[0];
+void h_28(double *state, double *unused, double *out_6922055836622213197) {
+   out_6922055836622213197[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_3803236046231543612) {
-   out_3803236046231543612[0] = 1;
-   out_3803236046231543612[1] = 0;
-   out_3803236046231543612[2] = 0;
-   out_3803236046231543612[3] = 0;
-   out_3803236046231543612[4] = 0;
-   out_3803236046231543612[5] = 0;
-   out_3803236046231543612[6] = 0;
-   out_3803236046231543612[7] = 0;
-   out_3803236046231543612[8] = 0;
+void H_28(double *state, double *unused, double *out_5948717585341275452) {
+   out_5948717585341275452[0] = 1;
+   out_5948717585341275452[1] = 0;
+   out_5948717585341275452[2] = 0;
+   out_5948717585341275452[3] = 0;
+   out_5948717585341275452[4] = 0;
+   out_5948717585341275452[5] = 0;
+   out_5948717585341275452[6] = 0;
+   out_5948717585341275452[7] = 0;
+   out_5948717585341275452[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_5594040852719868954) {
-   out_5594040852719868954[0] = state[8];
+void h_31(double *state, double *unused, double *out_3216953994560723966) {
+   out_3216953994560723966[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_1718755370106693421) {
-   out_1718755370106693421[0] = 0;
-   out_1718755370106693421[1] = 0;
-   out_1718755370106693421[2] = 0;
-   out_1718755370106693421[3] = 0;
-   out_1718755370106693421[4] = 0;
-   out_1718755370106693421[5] = 0;
-   out_1718755370106693421[6] = 0;
-   out_1718755370106693421[7] = 0;
-   out_1718755370106693421[8] = 1;
+void H_31(double *state, double *unused, double *out_3634840878481757515) {
+   out_3634840878481757515[0] = 0;
+   out_3634840878481757515[1] = 0;
+   out_3634840878481757515[2] = 0;
+   out_3634840878481757515[3] = 0;
+   out_3634840878481757515[4] = 0;
+   out_3634840878481757515[5] = 0;
+   out_3634840878481757515[6] = 0;
+   out_3634840878481757515[7] = 0;
+   out_3634840878481757515[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_3329877133651101623) {
-  err_fun(nom_x, delta_x, out_3329877133651101623);
+void car_err_fun(double *nom_x, double *delta_x, double *out_5959951015159528314) {
+  err_fun(nom_x, delta_x, out_5959951015159528314);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_7760882190653564987) {
-  inv_err_fun(nom_x, true_x, out_7760882190653564987);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_5356992719595228440) {
+  inv_err_fun(nom_x, true_x, out_5356992719595228440);
 }
-void car_H_mod_fun(double *state, double *out_1773647306661711299) {
-  H_mod_fun(state, out_1773647306661711299);
+void car_H_mod_fun(double *state, double *out_81214266615305410) {
+  H_mod_fun(state, out_81214266615305410);
 }
-void car_f_fun(double *state, double dt, double *out_8265395554005205627) {
-  f_fun(state,  dt, out_8265395554005205627);
+void car_f_fun(double *state, double dt, double *out_3378346814935937794) {
+  f_fun(state,  dt, out_3378346814935937794);
 }
-void car_F_fun(double *state, double dt, double *out_5121501708015469294) {
-  F_fun(state,  dt, out_5121501708015469294);
+void car_F_fun(double *state, double dt, double *out_4090498247982689107) {
+  F_fun(state,  dt, out_4090498247982689107);
 }
-void car_h_25(double *state, double *unused, double *out_1871114119926472838) {
-  h_25(state, unused, out_1871114119926472838);
+void car_h_25(double *state, double *unused, double *out_2941759932276218077) {
+  h_25(state, unused, out_2941759932276218077);
 }
-void car_H_25(double *state, double *unused, double *out_1749401331983653849) {
-  H_25(state, unused, out_1749401331983653849);
+void car_H_25(double *state, double *unused, double *out_8002552299589165215) {
+  H_25(state, unused, out_8002552299589165215);
 }
-void car_h_24(double *state, double *unused, double *out_4177328874243606255) {
-  h_24(state, unused, out_4177328874243606255);
+void car_h_24(double *state, double *unused, double *out_3100385683736896980) {
+  h_24(state, unused, out_3100385683736896980);
 }
-void car_H_24(double *state, double *unused, double *out_1808524266818669287) {
-  H_24(state, unused, out_1808524266818669287);
+void car_H_24(double *state, double *unused, double *out_8986618170430478883) {
+  H_24(state, unused, out_8986618170430478883);
 }
-void car_h_30(double *state, double *unused, double *out_6625546561337267154) {
-  h_30(state, unused, out_6625546561337267154);
+void car_h_30(double *state, double *unused, double *out_4392391324905101244) {
+  h_30(state, unused, out_4392391324905101244);
 }
-void car_H_30(double *state, double *unused, double *out_5167289009507962906) {
-  H_30(state, unused, out_5167289009507962906);
+void car_H_30(double *state, double *unused, double *out_3474855969461557017) {
+  H_30(state, unused, out_3474855969461557017);
 }
-void car_h_26(double *state, double *unused, double *out_8545349699624827873) {
-  h_26(state, unused, out_8545349699624827873);
+void car_h_26(double *state, double *unused, double *out_3042752109523599569) {
+  h_26(state, unused, out_3042752109523599569);
 }
-void car_H_26(double *state, double *unused, double *out_5490904650857710073) {
-  H_26(state, unused, out_5490904650857710073);
+void car_H_26(double *state, double *unused, double *out_4261048980715108991) {
+  H_26(state, unused, out_4261048980715108991);
 }
-void car_h_27(double *state, double *unused, double *out_3470753115959557725) {
-  h_27(state, unused, out_3470753115959557725);
+void car_h_27(double *state, double *unused, double *out_6523014969426492774) {
+  h_27(state, unused, out_6523014969426492774);
 }
-void car_H_27(double *state, double *unused, double *out_2992525697707537995) {
-  H_27(state, unused, out_2992525697707537995);
+void car_H_27(double *state, double *unused, double *out_5698450040645500234) {
+  H_27(state, unused, out_5698450040645500234);
 }
-void car_h_29(double *state, double *unused, double *out_7569056414238929589) {
-  h_29(state, unused, out_7569056414238929589);
+void car_h_29(double *state, double *unused, double *out_609281942755744401) {
+  h_29(state, unused, out_609281942755744401);
 }
-void car_H_29(double *state, double *unused, double *out_1279162970837986962) {
-  H_29(state, unused, out_1279162970837986962);
+void car_H_29(double *state, double *unused, double *out_3985087313775949201) {
+  H_29(state, unused, out_3985087313775949201);
 }
-void car_h_28(double *state, double *unused, double *out_6507539167403890410) {
-  h_28(state, unused, out_6507539167403890410);
+void car_h_28(double *state, double *unused, double *out_6922055836622213197) {
+  h_28(state, unused, out_6922055836622213197);
 }
-void car_H_28(double *state, double *unused, double *out_3803236046231543612) {
-  H_28(state, unused, out_3803236046231543612);
+void car_H_28(double *state, double *unused, double *out_5948717585341275452) {
+  H_28(state, unused, out_5948717585341275452);
 }
-void car_h_31(double *state, double *unused, double *out_5594040852719868954) {
-  h_31(state, unused, out_5594040852719868954);
+void car_h_31(double *state, double *unused, double *out_3216953994560723966) {
+  h_31(state, unused, out_3216953994560723966);
 }
-void car_H_31(double *state, double *unused, double *out_1718755370106693421) {
-  H_31(state, unused, out_1718755370106693421);
+void car_H_31(double *state, double *unused, double *out_3634840878481757515) {
+  H_31(state, unused, out_3634840878481757515);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
   predict(in_x, in_P, in_Q, dt);

selfdrive/locationd/models/generated/car.h

@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_3329877133651101623);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_7760882190653564987);
-void car_H_mod_fun(double *state, double *out_1773647306661711299);
-void car_f_fun(double *state, double dt, double *out_8265395554005205627);
-void car_F_fun(double *state, double dt, double *out_5121501708015469294);
-void car_h_25(double *state, double *unused, double *out_1871114119926472838);
-void car_H_25(double *state, double *unused, double *out_1749401331983653849);
-void car_h_24(double *state, double *unused, double *out_4177328874243606255);
-void car_H_24(double *state, double *unused, double *out_1808524266818669287);
-void car_h_30(double *state, double *unused, double *out_6625546561337267154);
-void car_H_30(double *state, double *unused, double *out_5167289009507962906);
-void car_h_26(double *state, double *unused, double *out_8545349699624827873);
-void car_H_26(double *state, double *unused, double *out_5490904650857710073);
-void car_h_27(double *state, double *unused, double *out_3470753115959557725);
-void car_H_27(double *state, double *unused, double *out_2992525697707537995);
-void car_h_29(double *state, double *unused, double *out_7569056414238929589);
-void car_H_29(double *state, double *unused, double *out_1279162970837986962);
-void car_h_28(double *state, double *unused, double *out_6507539167403890410);
-void car_H_28(double *state, double *unused, double *out_3803236046231543612);
-void car_h_31(double *state, double *unused, double *out_5594040852719868954);
-void car_H_31(double *state, double *unused, double *out_1718755370106693421);
+void car_err_fun(double *nom_x, double *delta_x, double *out_5959951015159528314);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_5356992719595228440);
+void car_H_mod_fun(double *state, double *out_81214266615305410);
+void car_f_fun(double *state, double dt, double *out_3378346814935937794);
+void car_F_fun(double *state, double dt, double *out_4090498247982689107);
+void car_h_25(double *state, double *unused, double *out_2941759932276218077);
+void car_H_25(double *state, double *unused, double *out_8002552299589165215);
+void car_h_24(double *state, double *unused, double *out_3100385683736896980);
+void car_H_24(double *state, double *unused, double *out_8986618170430478883);
+void car_h_30(double *state, double *unused, double *out_4392391324905101244);
+void car_H_30(double *state, double *unused, double *out_3474855969461557017);
+void car_h_26(double *state, double *unused, double *out_3042752109523599569);
+void car_H_26(double *state, double *unused, double *out_4261048980715108991);
+void car_h_27(double *state, double *unused, double *out_6523014969426492774);
+void car_H_27(double *state, double *unused, double *out_5698450040645500234);
+void car_h_29(double *state, double *unused, double *out_609281942755744401);
+void car_H_29(double *state, double *unused, double *out_3985087313775949201);
+void car_h_28(double *state, double *unused, double *out_6922055836622213197);
+void car_H_28(double *state, double *unused, double *out_5948717585341275452);
+void car_h_31(double *state, double *unused, double *out_3216953994560723966);
+void car_H_31(double *state, double *unused, double *out_3634840878481757515);
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
 void car_set_mass(double x);
 void car_set_rotational_inertia(double x);

selfdrive/locationd/models/generated/pose.h

@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void pose_err_fun(double *nom_x, double *delta_x, double *out_2803054085873220166);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_870718263858356541);
-void pose_H_mod_fun(double *state, double *out_245560868528396178);
-void pose_f_fun(double *state, double dt, double *out_8465405656783528291);
-void pose_F_fun(double *state, double dt, double *out_5153518595662752782);
-void pose_h_4(double *state, double *unused, double *out_3441983837194168135);
-void pose_H_4(double *state, double *unused, double *out_3792847385831294211);
-void pose_h_10(double *state, double *unused, double *out_6864246601547225854);
-void pose_H_10(double *state, double *unused, double *out_5871577252215708993);
-void pose_h_13(double *state, double *unused, double *out_3325053226247216097);
-void pose_H_13(double *state, double *unused, double *out_580573560498961410);
-void pose_h_14(double *state, double *unused, double *out_7213076452181803827);
-void pose_H_14(double *state, double *unused, double *out_170393470508190318);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_1596408993574171648);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4927362141599677432);
+void pose_H_mod_fun(double *state, double *out_7423446859784669653);
+void pose_f_fun(double *state, double dt, double *out_5202695727626385220);
+void pose_F_fun(double *state, double dt, double *out_5491057847988530682);
+void pose_h_4(double *state, double *unused, double *out_6452546827916926575);
+void pose_H_4(double *state, double *unused, double *out_2270928474745588918);
+void pose_h_10(double *state, double *unused, double *out_7382153153143119100);
+void pose_H_10(double *state, double *unused, double *out_8704391623487425268);
+void pose_h_13(double *state, double *unused, double *out_6604156015444971944);
+void pose_H_13(double *state, double *unused, double *out_941345350586743883);
+void pose_h_14(double *state, double *unused, double *out_3089011552067498211);
+void pose_H_14(double *state, double *unused, double *out_2706045001390472517);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

selfdrive/ui/layouts/settings/starpilot/lateral.py

@@ -108,6 +108,9 @@ class StarPilotLateralLayout(_SettingsPage):
     def nlc_on():
       return lc_on() and p.get_bool("NudgelessLaneChange")
 
+    def close_gap_on():
+      return lc_on() and p.get_bool("LaneChangeCloseGap")
+
     def pos_on():
       return p.get_bool("PauseLateralOnSignal")
 
@@ -191,6 +194,20 @@ class StarPilotLateralLayout(_SettingsPage):
         on_click=self._show_lane_smoothing,
         visible=lc_on,
       ),
+      SettingRow(
+        "LaneChangeCloseGap", "toggle", tr_noop("Close Gap On Lane Change"),
+        subtitle=tr_noop("Allows for a temporary shorter follow distance behind lead so that openpilot merges smoothly out of current lane, it will allow car to accelerate as it changes lanes."),
+        get_state=lambda: p.get_bool("LaneChangeCloseGap"),
+        set_state=lambda s: p.put_bool("LaneChangeCloseGap", s),
+        visible=lc_on,
+      ),
+      SettingRow(
+        "LaneChangeCloseGapSeconds", "value", tr_noop("Temporary Follow Distance"),
+        subtitle=tr_noop("Follow distance to hold while changing lanes. Only applied when shorter than your normal gap."),
+        get_value=self._get_lane_change_close_gap_display,
+        on_click=lambda: self._show_slider("LaneChangeCloseGapSeconds", 0.5, 3.0, step=0.05, unit="s", value_type="float"),
+        visible=close_gap_on,
+      ),
     ]
 
     # ── 3. Advanced Lateral Tuning ──
@@ -375,6 +392,9 @@ class StarPilotLateralLayout(_SettingsPage):
       return tr("Stock")
     return str(val)
 
+  def _get_lane_change_close_gap_display(self) -> str:
+    return f"{self._params.get_float('LaneChangeCloseGapSeconds'):.2f}s"
+
   def _show_lane_smoothing(self):
     def on_close(res, val):
       if res == DialogResult.CONFIRM:

selfdrive/ui/mici/onroad/augmented_road_view.py

@@ -19,7 +19,7 @@ from openpilot.selfdrive.ui.mici.onroad.starpilot_status import (
   TRAFFIC_COLOR,
   get_border_color,
 )
-from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
+from openpilot.selfdrive.ui.onroad.cameraview import CameraView
 from openpilot.selfdrive.ui.lib.starpilot_visuals import get_border_width
 from openpilot.starpilot.common.favorite_slots import is_favorite_action_key, load_favorite_slots, toggle_favorite_slot
 from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent
@@ -830,14 +830,16 @@ class AugmentedRoadView(CameraView):
 
   def _switch_stream_if_needed(self, sm, camera_view: int):
     if camera_view == CAMERA_VIEW_NONE:
+      self._cancel_pending_switch()
       self._reverse_driver_camera_frames = 0
       self._reverse_driver_camera_active = False
       return
 
+    if getattr(self, "_onroad_reentry_pending", False):
+      self._refresh_available_streams()
+
     if self._update_reverse_driver_camera_state():
-      target = DRIVER_CAM
-      if self.stream_type != target:
-        self.switch_stream(target)
+      self.switch_stream(DRIVER_CAM)
       return
 
     if camera_view == CAMERA_VIEW_DRIVER:
@@ -858,7 +860,8 @@ class AugmentedRoadView(CameraView):
     else:
       target = ROAD_CAM
 
-    if self.stream_type != target:
+    if (getattr(self, "_onroad_reentry_pending", False) or
+        self.stream_type != target or (self._switching and self._target_stream_type != target)):
       self.switch_stream(target)
 
   def _update_calibration(self):

selfdrive/ui/mici/onroad/cameraview.py

@@ -1,476 +1,5 @@
-import os
-import platform
-import weakref
-import numpy as np
-import pyray as rl
+"""Compatibility import for the shared CameraView implementation."""
 
-from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
-from openpilot.common.swaglog import cloudlog
-from openpilot.system.hardware import TICI
-from openpilot.system.ui.lib.application import gui_app
-from openpilot.system.ui.lib.egl import init_egl, create_egl_image, destroy_egl_image, bind_egl_image_to_texture, EGLImage
-from openpilot.system.ui.widgets import Widget
-from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
+from openpilot.selfdrive.ui.onroad.cameraview import CameraView
 
-CONNECTION_RETRY_INTERVAL = 0.2  # seconds between connection attempts
-MICI_FORCE_TEXTURE_CAMERA = os.getenv("MICI_FORCE_TEXTURE_CAMERA", "0") == "1"
-
-VERSION = """
-#version 300 es
-precision mediump float;
-"""
-if platform.system() == "Darwin":
-  VERSION = """
-    #version 330 core
-  """
-
-
-VERTEX_SHADER = VERSION + """
-in vec3 vertexPosition;
-in vec2 vertexTexCoord;
-in vec3 vertexNormal;
-in vec4 vertexColor;
-uniform mat4 mvp;
-out vec2 fragTexCoord;
-out vec4 fragColor;
-void main() {
-  fragTexCoord = vertexTexCoord;
-  fragColor = vertexColor;
-  gl_Position = mvp * vec4(vertexPosition, 1.0);
-}
-"""
-
-FRAME_FRAGMENT_SHADER_EXTERNAL = """
-  #version 300 es
-  #extension GL_OES_EGL_image_external_essl3 : enable
-  precision mediump float;
-  in vec2 fragTexCoord;
-  uniform samplerExternalOES texture0;
-  out vec4 fragColor;
-  uniform int engaged;
-  uniform int enhance_driver;
-
-  void main() {
-    vec4 color = texture(texture0, fragTexCoord);
-    // Keep the onroad camera feed full-color in every driving state.
-    if (engaged == 1) {
-      color.rgb = color.rgb;
-    }
-    if (enhance_driver == 1) {
-      float brightness = 1.1;
-      color.rgb = color.rgb + 0.15;
-      color.rgb = clamp((color.rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0);
-      color.rgb = color.rgb * color.rgb * (3.0 - 2.0 * color.rgb);
-      color.rgb = pow(color.rgb, vec3(0.8));
-    }
-    fragColor = vec4(color.rgb, color.a);
-  }
-  """
-
-FRAME_FRAGMENT_SHADER_YUV = VERSION + """
-  in vec2 fragTexCoord;
-  uniform sampler2D texture0;
-  uniform sampler2D texture1;
-  out vec4 fragColor;
-  uniform int engaged;
-  uniform int enhance_driver;
-
-  void main() {
-    float y = texture(texture0, fragTexCoord).r;
-    vec2 uv = texture(texture1, fragTexCoord).ra - 0.5;
-    vec3 rgb = vec3(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x);
-    // Keep the onroad camera feed full-color in every driving state.
-    if (engaged == 1) {
-      rgb = rgb;
-    }
-    // TODO: the images out of camerad need some more correction and
-    // the ui should apply a gamma curve for the device display
-    if (enhance_driver == 1) {
-      float brightness = 1.1;
-      rgb = rgb + 0.15;
-      rgb = clamp((rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0);
-      rgb = rgb * rgb * (3.0 - 2.0 * rgb);
-      rgb = pow(rgb, vec3(0.8));
-    }
-    fragColor = vec4(rgb, 1.0);
-  }
-  """
-
-
-class CameraView(Widget):
-  def __init__(self, name: str, stream_type: VisionStreamType):
-    super().__init__()
-    self._name = name
-    # Primary stream
-    self.client = VisionIpcClient(name, stream_type, conflate=True)
-    self._stream_type = stream_type
-    self.available_streams: list[VisionStreamType] = []
-
-    # Target stream for switching
-    self._target_client: VisionIpcClient | None = None
-    self._target_stream_type: VisionStreamType | None = None
-    self._switching: bool = False
-
-    self._texture_needs_update = True
-    self.last_connection_attempt: float = 0.0
-    self._use_egl = TICI and not MICI_FORCE_TEXTURE_CAMERA and init_egl()
-    if TICI and MICI_FORCE_TEXTURE_CAMERA:
-      cloudlog.warning("CameraView EGL disabled by MICI_FORCE_TEXTURE_CAMERA, using texture rendering")
-    elif TICI and not self._use_egl:
-      cloudlog.error("CameraView EGL init failed, falling back to texture rendering")
-
-    frame_shader = FRAME_FRAGMENT_SHADER_EXTERNAL if self._use_egl else FRAME_FRAGMENT_SHADER_YUV
-    self.shader = rl.load_shader_from_memory(VERTEX_SHADER, frame_shader)
-    self._texture1_loc: int = rl.get_shader_location(self.shader, "texture1") if not self._use_egl else -1
-    self._engaged_loc = rl.get_shader_location(self.shader, "engaged")
-    self._engaged_val = rl.ffi.new("int[1]", [1])
-    self._enhance_driver_loc = rl.get_shader_location(self.shader, "enhance_driver")
-    self._enhance_driver_val = rl.ffi.new("int[1]", [1 if stream_type == VisionStreamType.VISION_STREAM_DRIVER else 0])
-
-    self.frame: VisionBuf | None = None
-    self._last_frame_id = -1
-    self._regressive_frame_count = 0
-    self.texture_y: rl.Texture | None = None
-    self.texture_uv: rl.Texture | None = None
-
-    # EGL resources
-    self.egl_images: dict[int, EGLImage] = {}
-    self.egl_texture: rl.Texture | None = None
-
-    self._placeholder_color: rl.Color | None = None
-    self._closed = False
-
-    # Initialize EGL for zero-copy rendering when available.
-    if self._use_egl:
-      self._create_egl_texture()
-
-    self_ref = weakref.ref(self)
-
-    def offroad_transition_callback():
-      if (view := self_ref()) is not None:
-        view._offroad_transition()
-
-    self._offroad_transition_callback = offroad_transition_callback
-    ui_state.add_offroad_transition_callback(self._offroad_transition_callback)
-
-  def _offroad_transition(self):
-    self._reset_camera_connection()
-
-  def _reset_camera_connection(self):
-    # EGL images and VisionBuf objects both retain the imported camera buffer.
-    # Release them on every road-state transition instead of pinning the old
-    # camerad allocation until this view happens to render again.
-    self._clear_textures()
-    self.frame = None
-    self._last_frame_id = -1
-    self.available_streams.clear()
-    self.client = VisionIpcClient(self._name, self._stream_type, conflate=True)
-    self._target_client = None
-    self._target_stream_type = None
-    self._switching = False
-    self._texture_needs_update = True
-    self.last_connection_attempt = 0.0
-
-  def _set_placeholder_color(self, color: rl.Color):
-    """Set a placeholder color to be drawn when no frame is available."""
-    self._placeholder_color = color
-
-  def switch_stream(self, stream_type: VisionStreamType) -> None:
-    if self._stream_type == stream_type:
-      return
-
-    if self._switching and self._target_stream_type == stream_type:
-      return
-
-    cloudlog.debug(f'Preparing switch from {self._stream_type} to {stream_type}')
-
-    if self._target_client:
-      del self._target_client
-
-    self._target_stream_type = stream_type
-    self._target_client = VisionIpcClient(self._name, stream_type, conflate=True)
-    self._switching = True
-
-  @property
-  def stream_type(self) -> VisionStreamType:
-    return self._stream_type
-
-  def close(self) -> None:
-    if self._closed:
-      return
-    self._closed = True
-
-    callback = getattr(self, "_offroad_transition_callback", None)
-    if callback is not None:
-      ui_state.remove_offroad_transition_callback(callback)
-      self._offroad_transition_callback = None
-    self._clear_textures()
-
-    # Clean up shader
-    if self.shader and self.shader.id:
-      rl.unload_shader(self.shader)
-      self.shader.id = 0
-
-    self.frame = None
-    self._last_frame_id = -1
-    self.available_streams.clear()
-    self.client = None
-    self._target_client = None
-
-  def __del__(self):
-    self.close()
-
-  def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray:
-    if not self.frame:
-      return np.eye(3)
-
-    # Calculate aspect ratios
-    widget_aspect_ratio = rect.width / rect.height
-    frame_aspect_ratio = self.frame.width / self.frame.height
-
-    # Calculate scaling factors to maintain aspect ratio
-    zx = min(frame_aspect_ratio / widget_aspect_ratio, 1.0)
-    zy = min(widget_aspect_ratio / frame_aspect_ratio, 1.0)
-
-    return np.array([
-      [zx, 0.0, 0.0],
-      [0.0, zy, 0.0],
-      [0.0, 0.0, 1.0]
-    ])
-
-  def _render(self, rect: rl.Rectangle):
-    if self._switching:
-      self._handle_switch()
-
-    if not self._ensure_connection():
-      self._draw_placeholder(rect)
-      return
-
-    if self._use_egl:
-      self._observe_displayed_frame()
-
-    # Try to get a new buffer without blocking
-    buffer = self.client.recv(timeout_ms=0)
-    if buffer:
-      self._accept_frame(buffer, self.client.frame_id)
-    elif not self.client.is_connected():
-      # ensure we clear the displayed frame when the connection is lost
-      self.frame = None
-
-    if not self.frame:
-      self._draw_placeholder(rect)
-      return
-
-    transform = self._calc_frame_matrix(rect)
-    src_rect = rl.Rectangle(0, 0, float(self.frame.width), float(self.frame.height))
-    # Flip driver camera horizontally
-    if self._stream_type == VisionStreamType.VISION_STREAM_DRIVER:
-      src_rect.width = -src_rect.width
-
-    # Calculate scale
-    scale_x = rect.width * transform[0, 0]  # zx
-    scale_y = rect.height * transform[1, 1]  # zy
-
-    # Calculate base position (centered)
-    x_offset = rect.x + (rect.width - scale_x) / 2
-    y_offset = rect.y + (rect.height - scale_y) / 2
-
-    x_offset += transform[0, 2] * rect.width / 2
-    y_offset += transform[1, 2] * rect.height / 2
-
-    dst_rect = rl.Rectangle(x_offset, y_offset, scale_x, scale_y)
-
-    # Render with appropriate method
-    if self._use_egl:
-      self._render_egl(src_rect, dst_rect)
-    else:
-      self._render_textures(src_rect, dst_rect)
-
-  def _draw_placeholder(self, rect: rl.Rectangle):
-    if self._placeholder_color:
-      rl.draw_rectangle_rec(rect, self._placeholder_color)
-
-  def _observe_displayed_frame(self) -> None:
-    if self.frame is not None:
-      client_frame_id = getattr(self.client, "frame_id", -1) if hasattr(self, "client") and self.client is not None else -1
-      frame_id = getattr(self.frame, "frame_id", client_frame_id)
-      self._last_frame_id = max(self._last_frame_id, int(frame_id))
-
-  def _accept_frame(self, frame: VisionBuf, packet_frame_id: int) -> bool:
-    content_frame_id = int(getattr(frame, "frame_id", packet_frame_id))
-    if content_frame_id < self._last_frame_id:
-      self._regressive_frame_count += 1
-      if self._regressive_frame_count == 1 or self._regressive_frame_count % 100 == 0:
-        message = f"Dropping regressive {self._name} frame: content={content_frame_id}, packet={packet_frame_id}, "
-        message += f"displayed={self._last_frame_id}, idx={frame.idx}, count={self._regressive_frame_count}"
-        cloudlog.warning(message)
-      return False
-
-    self.frame = frame
-    self._last_frame_id = content_frame_id
-    self._texture_needs_update = True
-    return True
-
-  def _render_egl(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None:
-    """Render using EGL for direct buffer access"""
-    if self.frame is None or self.egl_texture is None:
-      return
-
-    idx = self.frame.idx
-    egl_image = self.egl_images.get(idx)
-
-    # Create EGL image if needed
-    if egl_image is None:
-      egl_image = create_egl_image(self.frame.width, self.frame.height, self.frame.stride, self.frame.fd, self.frame.uv_offset)
-      if egl_image:
-        self.egl_images[idx] = egl_image
-      else:
-        return
-
-    # Update texture dimensions to match current frame
-    self.egl_texture.width = self.frame.width
-    self.egl_texture.height = self.frame.height
-
-    # Bind the EGL image to our texture
-    bind_egl_image_to_texture(self.egl_texture.id, egl_image)
-
-    # Render with shader
-    rl.begin_shader_mode(self.shader)
-    self._update_texture_color_filtering()
-    rl.draw_texture_pro(self.egl_texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE)
-    rl.end_shader_mode()
-
-  def _render_textures(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None:
-    """Render using texture copies"""
-    if not self.texture_y or not self.texture_uv or self.frame is None:
-      return
-
-    # Update textures with new frame data
-    if self._texture_needs_update:
-      y_data = self.frame.data[: self.frame.uv_offset]
-      uv_data = self.frame.data[self.frame.uv_offset:]
-
-      rl.update_texture(self.texture_y, rl.ffi.cast("void *", y_data.ctypes.data))
-      rl.update_texture(self.texture_uv, rl.ffi.cast("void *", uv_data.ctypes.data))
-      self._texture_needs_update = False
-
-    # Render with shader
-    rl.begin_shader_mode(self.shader)
-    self._update_texture_color_filtering()
-    rl.set_shader_value_texture(self.shader, self._texture1_loc, self.texture_uv)
-    rl.draw_texture_pro(self.texture_y, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE)
-    rl.end_shader_mode()
-
-  def _update_texture_color_filtering(self):
-    self._engaged_val[0] = 1 if ui_state.status != UIStatus.DISENGAGED else 0
-    if self._engaged_loc >= 0:
-      rl.set_shader_value(self.shader, self._engaged_loc, self._engaged_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT)
-    if self._enhance_driver_loc >= 0:
-      rl.set_shader_value(self.shader, self._enhance_driver_loc, self._enhance_driver_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT)
-
-  def _ensure_connection(self) -> bool:
-    if not self.client.is_connected():
-      self.frame = None
-      self._last_frame_id = -1
-      self.available_streams.clear()
-
-      # Throttle connection attempts
-      current_time = rl.get_time()
-      if current_time - self.last_connection_attempt < CONNECTION_RETRY_INTERVAL:
-        return False
-      self.last_connection_attempt = current_time
-
-      # A GL texture can retain the last EGL image after camerad exits. Release
-      # it before connect() frees and replaces the client's imported buffers.
-      self._clear_textures()
-      if not self.client.connect(False) or not self.client.num_buffers:
-        return False
-
-      cloudlog.debug(f"Connected to {self._name} stream: {self._stream_type}, buffers: {self.client.num_buffers}")
-      self._initialize_textures()
-      self.available_streams = self.client.available_streams(self._name, block=False)
-
-    return True
-
-  def _handle_switch(self) -> None:
-    """Check if target stream is ready and switch immediately."""
-    if not self._target_client or not self._switching:
-      return
-
-    # Try to connect target if needed
-    if not self._target_client.is_connected():
-      if not self._target_client.connect(False) or not self._target_client.num_buffers:
-        return
-
-      cloudlog.debug(f"Target stream connected: {self._target_stream_type}")
-
-    # Check if target has frames ready
-    target_frame = self._target_client.recv(timeout_ms=0)
-    if target_frame:
-      self.frame = target_frame  # Update current frame to target frame
-      self._complete_switch()
-
-  def _complete_switch(self) -> None:
-    """Instantly switch to target stream."""
-    cloudlog.debug(f"Switching to {self._target_stream_type}")
-    # Delete the GL texture before releasing the old client. Merely destroying
-    # the EGLImage handle leaves its storage alive while a texture sibling exists.
-    self._clear_textures()
-
-    # Switch to target
-    self.client = self._target_client
-    self._stream_type = self._target_stream_type
-    client_frame_id = getattr(self.client, "frame_id", -1) if hasattr(self, "client") and self.client is not None else -1
-    self._last_frame_id = int(getattr(self.frame, "frame_id", client_frame_id)) if self.frame is not None else -1
-    self._texture_needs_update = True
-
-    # Reset state
-    self._target_client = None
-    self._target_stream_type = None
-    self._switching = False
-
-    # Initialize textures for new stream
-    self._initialize_textures()
-
-  def _initialize_textures(self):
-    self._clear_textures()
-    if self._use_egl:
-      self._create_egl_texture()
-    else:
-      self.texture_y = rl.load_texture_from_image(rl.Image(None, int(self.client.stride),
-        int(self.client.height), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAYSCALE))
-      self.texture_uv = rl.load_texture_from_image(rl.Image(None, int(self.client.stride // 2),
-        int(self.client.height // 2), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAY_ALPHA))
-
-  def _create_egl_texture(self):
-    # A fresh texture has no EGL image sibling from a previous camera client.
-    temp_image = rl.gen_image_color(1, 1, rl.BLACK)
-    self.egl_texture = rl.load_texture_from_image(temp_image)
-    rl.unload_image(temp_image)
-
-  def _clear_textures(self):
-    if self.texture_y and self.texture_y.id:
-      rl.unload_texture(self.texture_y)
-      self.texture_y = None
-
-    if self.texture_uv and self.texture_uv.id:
-      rl.unload_texture(self.texture_uv)
-      self.texture_uv = None
-
-    # Delete the texture first. eglDestroyImageKHR only destroys the EGLImage
-    # handle; the image storage stays alive while a GL texture sibling exists.
-    if self._use_egl:
-      if self.egl_texture and self.egl_texture.id:
-        rl.unload_texture(self.egl_texture)
-      self.egl_texture = None
-
-      for data in self.egl_images.values():
-        destroy_egl_image(data)
-      self.egl_images = {}
-
-
-if __name__ == "__main__":
-  gui_app.init_window("camera view")
-  road = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD)
-  for _ in gui_app.render():
-    road.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
+__all__ = ["CameraView"]

selfdrive/ui/mici/onroad/driver_camera_dialog.py

@@ -1,7 +1,7 @@
 import pyray as rl
 from cereal import car, log, messaging
 from msgq.visionipc import VisionStreamType
-from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
+from openpilot.selfdrive.ui.onroad.cameraview import CameraView
 from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
 from openpilot.selfdrive.ui.ui_state import ui_state, device
 from openpilot.system.ui.lib.application import gui_app, FontWeight

selfdrive/ui/mici/tests/test_camera_cleanup.py

@@ -2,14 +2,13 @@ import gc
 from types import SimpleNamespace
 import weakref
 
-import pytest
-
-from openpilot.selfdrive.ui.mici.onroad import cameraview as mici_cameraview
 from openpilot.selfdrive.ui.onroad import cameraview as big_cameraview
+from openpilot.selfdrive.ui.mici.onroad import augmented_road_view as mici_augmented_road_view
 
 
-@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview))
-def test_road_transition_releases_camera_buffers(monkeypatch, module):
+def test_road_transition_releases_camera_buffers(monkeypatch):
+  module = big_cameraview
+
   class FakeClient:
     pass
 
@@ -25,6 +24,7 @@ def test_road_transition_releases_camera_buffers(monkeypatch, module):
   view._target_stream_type = object()
   view._switching = True
   view._texture_needs_update = False
+  view._regressive_frame_count = 2
   view.last_connection_attempt = 123.0
   view._closed = True
   cleared = []
@@ -44,11 +44,13 @@ def test_road_transition_releases_camera_buffers(monkeypatch, module):
   assert view._target_stream_type is None
   assert view._switching is False
   assert view._texture_needs_update
+  assert view._regressive_frame_count == 0
   assert view.last_connection_attempt == 0.0
 
 
-@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview))
-def test_transition_callback_does_not_retain_camera_view(monkeypatch, module):
+def test_transition_callback_does_not_retain_camera_view(monkeypatch):
+  module = big_cameraview
+
   class FakeClient:
     pass
 
@@ -72,8 +74,9 @@ def test_transition_callback_does_not_retain_camera_view(monkeypatch, module):
   assert callbacks == []
 
 
-@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview))
-def test_stream_switch_releases_graphics_before_old_client(module):
+def test_stream_switch_releases_graphics_before_old_client():
+  module = big_cameraview
+
   events = []
 
   class FakeClient:
@@ -86,6 +89,8 @@ def test_stream_switch_releases_graphics_before_old_client(module):
   view._target_stream_type = object()
   view._stream_type = object()
   view._switching = True
+  view.frame = SimpleNamespace(frame_id=10)
+  view._regressive_frame_count = 2
   view._texture_needs_update = False
   view._closed = True
   view._clear_textures = lambda: events.append("graphics")
@@ -96,10 +101,12 @@ def test_stream_switch_releases_graphics_before_old_client(module):
 
   assert old_client_finalizer.alive is False
   assert events == ["graphics", "client", "initialize"]
+  assert view._regressive_frame_count == 0
 
 
-@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview))
-def test_egl_cleanup_deletes_texture_before_images(monkeypatch, module):
+def test_egl_cleanup_deletes_texture_before_images(monkeypatch):
+  module = big_cameraview
+
   events = []
   view = module.CameraView.__new__(module.CameraView)
   view.texture_y = None
@@ -108,10 +115,7 @@ def test_egl_cleanup_deletes_texture_before_images(monkeypatch, module):
   view.egl_images = {0: object(), 1: object()}
   view._closed = True
 
-  if module is mici_cameraview:
-    view._use_egl = True
-  else:
-    monkeypatch.setattr(module, "TICI", True)
+  view._use_egl = True
 
   monkeypatch.setattr(module.rl, "unload_texture", lambda _texture: events.append("texture"))
   monkeypatch.setattr(module, "destroy_egl_image", lambda _image: events.append("image"))
@@ -121,3 +125,73 @@ def test_egl_cleanup_deletes_texture_before_images(monkeypatch, module):
   assert events == ["texture", "image", "image"]
   assert view.egl_texture is None
   assert view.egl_images == {}
+
+
+def test_egl_cleanup_synchronizes_after_backend_switch(monkeypatch):
+  module = big_cameraview
+
+  events = []
+  view = module.CameraView.__new__(module.CameraView)
+  view.texture_y = None
+  view.texture_uv = None
+  view.egl_texture = SimpleNamespace(id=7)
+  view.egl_images = {0: object()}
+  view._use_egl = False
+  view._closed = True
+
+  monkeypatch.setattr(module, "is_egl_initialized", lambda: True)
+  monkeypatch.setattr(module.rl, "rl_draw_render_batch_active", lambda: events.append("flush"))
+  monkeypatch.setattr(module, "finish_gl", lambda: events.append("finish"))
+  monkeypatch.setattr(module.rl, "unload_texture", lambda _texture: events.append("texture"))
+  monkeypatch.setattr(module, "destroy_egl_image", lambda _image: events.append("image"))
+
+  view._clear_textures()
+
+  assert events == ["flush", "finish", "texture", "image"]
+
+
+def test_reverse_activation_cancels_mismatched_pending_switch():
+  view = mici_augmented_road_view.AugmentedRoadView.__new__(mici_augmented_road_view.AugmentedRoadView)
+  view._stream_type = mici_augmented_road_view.DRIVER_CAM
+  view._target_stream_type = mici_augmented_road_view.WIDE_CAM
+  view._target_client = object()
+  view._switching = True
+  view._closed = True
+  view._update_reverse_driver_camera_state = lambda: True
+
+  view._switch_stream_if_needed(None, mici_augmented_road_view.CAMERA_VIEW_AUTO)
+
+  assert view._target_client is None
+  assert view._target_stream_type is None
+  assert not view._switching
+
+
+def test_onroad_transition_marks_camera_reentry(monkeypatch):
+  module = big_cameraview
+
+  class FakeClient:
+    pass
+
+  view = module.CameraView.__new__(module.CameraView)
+  view._name = "camerad"
+  view._stream_type = object()
+  view.client = FakeClient()
+  view.frame = None
+  view.available_streams = []
+  view._target_client = None
+  view._target_stream_type = None
+  view._switching = False
+  view._texture_needs_update = False
+  view._regressive_frame_count = 1
+  view._closed = True
+  view._onroad_reentry_pending = False
+  view._reentry_stream_selected = False
+  view._clear_textures = lambda: None
+
+  monkeypatch.setattr(module, "VisionIpcClient", lambda *_args, **_kwargs: FakeClient())
+  monkeypatch.setattr(module.ui_state, "is_onroad", lambda: True)
+
+  view._offroad_transition()
+
+  assert view._onroad_reentry_pending
+  assert not view._reentry_stream_selected

selfdrive/ui/onroad/augmented_road_view.py

@@ -200,10 +200,14 @@ class AugmentedRoadView(CameraView):
 
   def _switch_stream_if_needed(self, sm, camera_view: int):
     if camera_view == CAMERA_VIEW_NONE:
+      self._cancel_pending_switch()
       self._reverse_driver_camera_frames = 0
       self._reverse_driver_camera_active = False
       return
 
+    if getattr(self, "_onroad_reentry_pending", False):
+      self._refresh_available_streams()
+
     if self._update_reverse_driver_camera_state():
       target = DRIVER_CAM
     elif camera_view == CAMERA_VIEW_DRIVER:
@@ -224,7 +228,8 @@ class AugmentedRoadView(CameraView):
     else:
       target = ROAD_CAM
 
-    if self.stream_type != target:
+    if (getattr(self, "_onroad_reentry_pending", False) or
+        self.stream_type != target or (self._switching and self._target_stream_type != target)):
       self.switch_stream(target)
 
   def _update_calibration(self):