modeld_v2: count the crawl-turn corner by travel, hold 1.5 m past it, and a toggle to turn it off

Route 92, the first drive with the turn-in hold: on both slow right turns (2-4 mph) the car was held straight for about 4 s until the driver steered. Held straight, the model re-planned its corner 0.5-1.2 m ahead of the bumper frame after frame, so the plan's corner never reached the bumper and the hold never let go. The open-loop replay of routes 85/89 couldn't show this, because those logged cars were turning. Once the plan's corner is within 3 m it's now counted down by travel (or the plan's own distance, if that's closer). Counted that way, the old turn-ins came about 1 m past the corner and the driver gave up on route 92 about 2.5 m past it, so the hold now lasts 1.5 m past the corner (was 2 m), letting go over its last metre, and 2 s at most (was 2.5 s). Replaying the new shaper on route 92, it lets go at 54.95 s and 752.5 s, before the driver took the wheel at 55.6 s and 753.0 s. On routes 85/89 it still holds 29 of the 71 pushback frames under 4 m/s. Adds a TurnInHold toggle ("late turn-in" on the comma four Models screen, custom models only, on by default). Off, nothing is held and the full-plan floor acts from the start of the turn. Rebuilds the prebuilt ARM64 libparams_c.so with the repo's params rebuild tool so the new key is registered. Compared in the ARM64 container, the registry differs from the previous library only by TurnInHold (bool, default 1, persistent, backup). Not yet run on the device. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
2026-11-07 09:53:43 +08:00 · 2026-11-06 15:00:54 -05:00
parent b9d8f95ae9
commit f6a08afbf0
10 changed files with 96 additions and 29 deletions
@@ -15,11 +15,12 @@ Only during a signalled turn (turn desire), and only at a crawl: full under 4 m/
 1. **Turn-in hold.** The plan's corner is where its heading has turned 5 deg toward the turn, measured along the plan's
   positions from the front bumper (camera + 1.52 m). While the plan has a corner ahead, steering toward the turn is
   held to the road's curvature from before the corner (the plan's curvature under the car, last taken while the corner
-   was still 3 m or more away), so a curving approach is still followed. Once the corner reaches the bumper, the hold
-   lasts another 2 m of travel, counted by speed (a car held at the corner gets a plan that turns right where it is),
-   letting go over the last metre. Then it's off for that turn. Steering away from the turn is never touched; a plan
-   with no corner is never held; waiting at a stop doesn't count; creeping toward a corner counts for 8 s at most; past
-   the corner it lasts 2.5 s at most.
+   was still 3 m or more away), so a curving approach is still followed. Once the corner is within 3 m it's counted
+   down by travel (or the plan's own distance, if that's closer), since a car held straight gets a plan whose corner
+   stays just ahead. When it reaches the bumper, the hold lasts another 1.5 m of travel, letting go over the last
+   metre. Then it's off for that turn. Steering away from the turn is never touched; a plan with no corner is never
+   held; waiting at a stop doesn't count; creeping toward a corner counts for 8 s at most; past the corner it lasts
+   2 s at most. The `TurnInHold` toggle ("late turn-in" on the comma four Models screen, on by default) turns it off.
 2. **Harder turn with look-ahead.** The turn plan floor is the full plan (1.0x at <=4 m/s, fading to 0 by 8 m/s, up from
   0.75x), read 0.3 s further ahead at <=4 m/s (fading out by 8 m/s) so the rate-limited wheel arrives on time. It's off
   while the hold is on, and the model's own turn state gates it as before.
@@ -40,6 +41,16 @@ The first version, released when the corner came within 1 m, didn't move the tur
 pushback frames after release). The built version holds 55 of 71 pushback frames under 4 m/s, delays the turn-in a
 median 2.0 m (0-7.5 m), and holds back 0.04-0.10 1/m at crawl turn-ins; faster turns are untouched.

+## Route 92, the first drive with the hold
+
+Closed loop it failed at a crawl: held straight, the model re-planned its corner 0.5-1.2 m ahead of the bumper for 3-4 s
+on both slow right turns (2-4 mph), so the corner never arrived and the car stayed straight until the driver steered.
+The open-loop replay couldn't show it, because those logged cars were turning. Counted by travel instead, the old
+turn-ins (routes 85/89) came about 1 m past the corner and the driver gave up on route 92 about 2.5 m past it. At
+1.5 m the hold would have let go 0.5-0.65 s before the driver took over on route 92, and on routes 85/89 it still holds
+29 of the 71 pushback frames (2 m: 0.2-0.4 s and 34; 1 m: 0.75-1 s and 17). The turns at 13-19 mph were fine. A left at
+11-15 mph was too wide: the model's own turn was about a 33 m radius, and the floor is half strength or less there.
+
 ## Logging

 modelDataV2SP: turnInHold, turnBendDistance, turnLead, turnRouteRemainingDeg, turnRouteFactor.
@@ -267,6 +267,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"Blendful", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
    {"PlanLong", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"TurnInHold", {PERSISTENT | BACKUP, BOOL, "1"}},

    // mapd
    {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}},
@@ -114,6 +114,11 @@ class ModelsLayoutMici(NavScroller):
                                                           "while the model still sets the cruising speed. Turns off the accel boost. " +
                                                           "Needs openpilot longitudinal. Not available with the Default model."))

+    self.turn_in_hold_toggle = BigParamControl(tr("late turn-in"), "TurnInHold",
+                                               description=tr("On slow signalled turns, drives about 1.5 m past the corner before turning, " +
+                                                              "so the inside wheels clear it. Off, it turns in where the model wants. " +
+                                                              "Needs lane turn desire. Not available with the Default model."))
+
    self.map_turn_toggle = BigParamControl(tr("signal navigation (experimental)"), "MapboxNextTurnEnabled",
      toggle_callback=self._enable_map_turns,
      description=tr("Signals can prepare a short street route while normal lane-change checks remain available. " +
@@ -124,7 +129,7 @@ class ModelsLayoutMici(NavScroller):

    self.main_items = [self.current_model_info, self.preparation_details_btn, self.select_model_btn, self.prepare_navigation_btn, self.cancel_download_btn,
                       self.refresh_btn, self.clear_cache_btn,
-                       self.blendful_toggle, self.plan_long_toggle, self.map_turn_toggle]
+                       self.blendful_toggle, self.plan_long_toggle, self.turn_in_hold_toggle, self.map_turn_toggle]
    self._scroller.add_widgets(self.main_items)

  def _prepare_navigation_model(self):
@@ -278,6 +283,7 @@ class ModelsLayoutMici(NavScroller):
    super().show_event()
    self.blendful_toggle.refresh()
    self.plan_long_toggle.refresh()
+    self.turn_in_hold_toggle.refresh()

  def _update_state(self):
    super()._update_state()
@@ -304,6 +310,11 @@ class ModelsLayoutMici(NavScroller):
    self.plan_long_toggle.set_enabled(blendful_allowed)
    if self.plan_long_toggle.value != blendful_value:
      self.plan_long_toggle.set_value(blendful_value)
+    # the turn-in hold lives in the custom model's modeld too
+    self.turn_in_hold_toggle.set_checked(blendful_allowed and ui_state.params.get_bool("TurnInHold"))
+    self.turn_in_hold_toggle.set_enabled(blendful_allowed)
+    if self.turn_in_hold_toggle.value != blendful_value:
+      self.turn_in_hold_toggle.set_value(blendful_value)

    manager = self.model_manager
    self._download_frame += 1
@@ -481,6 +481,7 @@ def main(demo=False):
      plan_long_enabled = params.get_bool("PlanLong")
      if not plan_long_enabled:
        plan_long.reset()
+      model.turn_shaper.hold_enabled = params.get_bool("TurnInHold")
    lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
    if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
      device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32)
@@ -61,7 +61,7 @@ def test_scale_is_full_at_low_speed_and_fades_out():

 def _run_action(turn_sign, v_ego=2., k=0.1, action_share=0.5, turn_state=None, corner_m=0., route_deg=None, turn_deg=None,
                frames=40):
-  # frames: model frames run on the same plan; the default carries the car past the corner hold (2 m at 2 m/s is 20)
+  # frames: model frames run on the same plan; the default carries the car past the corner hold (1.5 m at 2 m/s is 15)
  # a right turn the plan holds at curvature k from corner_m metres out while the action head asks for only part of it
  from typing import Any
  import numpy as np
@@ -147,7 +147,7 @@ def test_before_the_corner_the_turn_in_is_held_and_the_floor_is_off(sign):


@pytest.mark.parametrize("sign", [RIGHT, LEFT])
-def test_at_the_corner_the_turn_in_waits_2_m_then_the_floor_is_the_full_plan(sign):
+def test_at_the_corner_the_turn_in_waits_then_the_floor_is_the_full_plan(sign):
  state, action = _run_action(sign, k=sign * 0.1, corner_m=0., frames=1)
  assert state.turn_shaper.shape.hold == 1.
  assert action.desiredCurvature == pytest.approx(0., abs=1e-6)
@@ -87,18 +87,36 @@ def drive(shaper, plan, v, frames, sign=RIGHT):
  return [shaper.update(*plan, sign, v).hold for _ in range(frames)]


-def test_shaper_holds_to_the_corner_then_2_m_past_it_then_lets_go_for_the_turn():
+def test_shaper_holds_to_the_corner_then_past_it_then_lets_go_for_the_turn():
  shaper = ts.TurnShaper()
  assert drive(shaper, corner_plan(12.), 3., 5) == [1.] * 5
-  # the corner reaches the bumper: still held for the next 2 m (0.15 m a frame at 3 m/s)
+  # the corner reaches the bumper: still held for the next 1.5 m, letting go over its last metre (0.15 m a frame at 3 m/s)
  holds = drive(shaper, corner_plan(0.), 3., 20)
-  assert holds[:6] == [1.] * 6
+  assert holds[:3] == [1.] * 3
  assert holds[-1] == 0. and shaper.released
  assert holds == sorted(holds, reverse=True)
  # turning, the plan's corner can look further off again: the hold doesn't come back
  assert drive(shaper, corner_plan(12.), 3., 3) == [0.] * 3


+def test_a_corner_the_plan_keeps_just_ahead_is_still_reached_by_travel():
+  # route 92: held straight at a crawl, the model re-plans its corner about 1 m ahead of the bumper, frame after frame
+  plan = corner_plan(1.8)
+  assert ts.plan_bend_distance(*plan, RIGHT) == pytest.approx(1., abs=0.1)
+  shaper = ts.TurnShaper()
+  holds = drive(shaper, plan, 2., 30)  # 0.1 m a frame: the corner after about 10, then 1.5 m more
+  assert holds[:14] == [1.] * 14
+  assert holds[-1] == 0. and shaper.released
+
+
+def test_the_hold_can_be_turned_off_and_the_corner_is_still_measured():
+  shaper = ts.TurnShaper()
+  shaper.hold_enabled = False
+  shape = shaper.update(*corner_plan(12.), RIGHT, 3.)
+  assert shape.hold == 0.
+  assert shape.bend == pytest.approx(12. + ts.BEND_RAD / 0.12 - ts.CAMERA_TO_FRONT, abs=0.3)
+
+
 def test_waiting_at_the_corner_still_turns_in_after_hold_max_s():
  shaper = ts.TurnShaper()
  drive(shaper, corner_plan(0.), 3., 1)
@@ -15,6 +15,11 @@ corner 1.4-3.5 m ahead of the camera), mostly while the floor set the command; a
 those turns, the car already turned in about when the plan's corner reached the bumper (69 of 71 pushback frames came
 after that), so the hold has to last past the corner: a further PAST_CORNER_M of travel, counted by speed, since a car
 held at the corner gets a plan that turns right where it is.
+
+Route 92, the first drive with the hold: held straight at a crawl, the model kept re-planning its corner 0.5-1.2 m ahead
+of the bumper, so it never arrived and two right turns stayed held for about 4 s until the driver steered. So once the
+corner is within NEAR_CORNER_M it's counted down by travel. Counted that way, the old turn-ins came about 1 m past it
+and the driver gave up on route 92 about 2.5 m past it, hence PAST_CORNER_M of 1.5 m.
 """
 import math
 from dataclasses import dataclass
@@ -26,15 +31,15 @@ BEND_RAD = math.radians(5.)  # the plan's corner starts where its heading has tu
 TURN_IN_AT = 0.  # m ahead of the front bumper: the plan's corner is reached here
 APPROACH_MAX_S = 8.  # s of creeping toward the corner, at most: a plan whose corner stays just out of reach can't pin it
 MOVING = 0.5  # m/s: waiting at a stop doesn't use up the approach
-PAST_CORNER_M = 2.0  # m travelled past the corner before turning in, like FSD going well past the entrance
+PAST_CORNER_M = 1.5  # m travelled past the corner before turning in, like FSD going past the entrance
 PAST_CORNER_RAMP = 1.0  # m over which the hold lets go at the end of that
-HOLD_MAX_S = 2.5  # s after reaching the corner, whatever the travel: the hold never outlasts this
+HOLD_MAX_S = 2.0  # s after reaching the corner, whatever the travel: the hold never outlasts this
 DT = 0.05  # s between updates, the model's frame
 HOLD_SPEED_BP = [4., 6.]  # m/s: full hold at a crawl, none from 13 mph, where turns were fine
 MIN_PLAN_TRAVEL = 8.  # m: a plan covering less (a stopped car) has no corner to measure
 LEAD_BP, LEAD_V = [4., 8.], [0.3, 0.]  # s of extra look-ahead for the floor: the wheel lags its command most at a crawl
 NOW_T = 0.1  # s: reading the plan this soon gives the curvature of the road right under the car
-NEAR_CORNER_M = 3.  # m: from this close the plan's curvature under the car is the turn itself, not the road before it
+NEAR_CORNER_M = 3.  # m: from this close the plan's curvature under the car is the turn, and the corner is counted by travel
 ROUTE_MARGIN_DEG = 10.  # the route has to want this much more turn than the plan before it counts
 ROUTE_MIN_PLAN_DEG = 30.  # a plan that has barely started its turn isn't scaled from almost nothing
 ROUTE_MAX_FACTOR = 1.3
@@ -154,10 +159,12 @@ class TurnShaper:
  def __init__(self):
    self.reset(0.)
    self.shape = TurnShape()
+    self.hold_enabled = True  # the TurnInHold toggle; off, nothing is held and the floor acts from the start

  def reset(self, turn_sign: float) -> None:
    self.sign = turn_sign
-    self.reached = False  # the plan's corner has come to the bumper
+    self.reached = False  # the corner has come to the bumper
+    self.corner_left: float | None = None  # m from the bumper to the corner, counted by travel once within NEAR_CORNER_M
    self.approach_s = 0.  # s held before the corner
    self.past_m, self.held_s = 0., 0.  # travel and time since the corner
    self.road = 0.  # the road's curvature under the car while the corner was still NEAR_CORNER_M away
@@ -174,8 +181,14 @@ class TurnShaper:
    bend = plan_bend_distance(xs, ys, yaws, turn_sign)
    if not self.reached and (bend is None or bend > NEAR_CORNER_M):
      self.road = road_now
+    # a near corner is counted down by travel: held straight, the model keeps re-planning its corner just ahead, so the
+    # plan's own corner would never arrive. A plan whose corner comes closer than that still counts.
+    if self.corner_left is not None:
+      self.corner_left -= max(v_ego, 0.) * DT
+    if bend is not None and (self.corner_left is not None or bend <= NEAR_CORNER_M):
+      self.corner_left = bend if self.corner_left is None else min(self.corner_left, bend)
    # only the corner starts the countdown: a turn signalled at speed still gets held once it slows for the corner
-    if not self.reached and bend is not None and bend <= TURN_IN_AT:
+    if not self.reached and self.corner_left is not None and self.corner_left <= TURN_IN_AT:
      self.reached = True
    if self.reached and not self.released:
      self.past_m += max(v_ego, 0.) * DT
@@ -190,7 +203,7 @@ class TurnShaper:
      hold = approach_hold(bend, self.approach_s)
      if hold > 0. and MOVING < v_ego and speed_factor(v_ego) > 0.:
        self.approach_s += DT
-    self.shape = TurnShape(hold=hold * speed_factor(v_ego), lead=turn_lead(v_ego),
+    self.shape = TurnShape(hold=hold * speed_factor(v_ego) if self.hold_enabled else 0., lead=turn_lead(v_ego),
                           factor=route_factor(route_remaining, yaws, turn_sign), bend=-1. if bend is None else bend,
                           route_deg=-1. if route_remaining is None else route_remaining, road=self.road)
    return self.shape
@@ -23,7 +23,9 @@ The previous build preserved all 273 base key names, types, flags and raw defaul
 and added five Mapbox keys. The navigation build adds only `NavModelState`;
 all 278 previous registry entries were compared against the real ARM64 library
 and remain identical. The plan braking build adds only the `PlanLong` bool (persistent, backup, default
-`0`); all 279 previous registry entries were compared against the real ARM64 library and remain identical.
+`0`); all 279 previous registry entries were compared against the real ARM64 library and remain identical. The late
+turn-in build adds only the `TurnInHold` bool (persistent, backup, default `1`); all 280 previous registry entries were
+compared against the real ARM64 library and remain identical.
 The status JSON clears on manager startup and going offroad. The C API exports and dynamic
 library dependencies match the base release, with no increased symbol-version
 requirements. The `params_keys_by_flag` correction returns distinct buffers
@@ -1,11 +1,11 @@
 {
  "schemaVersion": 1,
-  "baseCommit": "c425b51ba6732b395d06ebd787751321d0cde4ba",
+  "baseCommit": "b9d8f95ae98b7c3d3683d747583c00a9f00cb564",
  "sourceTreeIncludesFeatureChanges": true,
  "builder": {
    "architecture": "native aarch64 Linux",
    "baseImage": "debian:bookworm-slim@sha256:99c0b25c46d0214d5a2091092e7908907dcaecd6a962cde0ac700acee222b6f3",
-    "testedImageId": "sha256:ce668bac08f6454af65345eb76af47619a50f978b7c25d040e96febff4b8f631",
+    "testedImageId": "sha256:abda66d2bbcc53009489cd6b4797ef3ab398b57519c72f8013f27b695096de38",
    "compiler": "g++ (Debian 12.2.0-14+deb12u1) 12.2.0",
    "compileAndTestNetwork": "none",
    "repositoryMountedInContainer": false
@@ -14,7 +14,7 @@
    "openpilot/common/params.h": "3c7b87d050c79275a401597dc7d15ae9fa57c2316c816ff5472cbdd84588a6e6",
    "openpilot/common/params.cc": "0074bdf766e9bf410f21820b8f730ed9403117c54725b6193176f28a14233737",
    "openpilot/common/params_c.cc": "7c3c86301ab3d65aa0eecc3f7b61c3baf4730baf72a9166cbb4de4237419a48d",
-    "openpilot/common/params_keys.h": "768739177e7e0798d4ca1c4af10c82293127aa9a733f3e16e0817a46c8187032",
+    "openpilot/common/params_keys.h": "08a3103a245bc440a8db585ff5f0a486c2f6621925712f27ff40ef5d8d7bb712",
    "openpilot/common/queue.h": "f7c75c847ae63dfd6e864406e722248540a79908270e85ca2674b43407408fcf",
    "openpilot/common/timing.h": "66a580e240fa4fe4af03bc3073ddd0f4db5d1027720e8adfa92ba448f7c8bc71",
    "openpilot/common/util.cc": "4479ecf72465e8f453d8af78447f7715f02d9397c58a49048f2bbc87a96d6b8a",
@@ -57,7 +57,7 @@
  ],
  "rebuildTools": {
    "Dockerfile": "c60c96043ea7b6d8ad94ab4e9181033606ae35e4fb43e034ab38b7505d867cff",
-    "README.md": "50f87b6e127db3d43f5f85e2124f98e99f4c576bc8fb561e6bdddf6e309598bd",
+    "README.md": "c6c2467839e05fffed20a60a666ad95cdfcb68659f5d2cd1b043e28e272246a1",
    "verify.py": "b068578579c1694c7b1594deca244917894ba43d69a80651058280c9a60a1b9f",
    "rebuild.py": "62f25ff2e6bd7daa9a635d6a6f49b7fdf03533370f2f75296d77003bed01d765"
  },
@@ -94,8 +94,8 @@
    ],
    "compiler": "g++ (Debian 12.2.0-14+deb12u1) 12.2.0",
    "machine": "aarch64",
-    "sha256": "fcd1a9af21eaa5da8f6e487f1864b95f85cae6e5babd1e70563a1a2892688ad0",
-    "bytes": 4142344
+    "sha256": "cc59d319b431523db077d81a0272df55987c0d56fd594d530adfc37f0acc29df",
+    "bytes": 4147088
  },
  "verification": {
    "originalTestMethods": [
@@ -181,7 +181,7 @@
      ]
    },
    "rebuilt": {
-      "sha256": "fcd1a9af21eaa5da8f6e487f1864b95f85cae6e5babd1e70563a1a2892688ad0",
+      "sha256": "cc59d319b431523db077d81a0272df55987c0d56fd594d530adfc37f0acc29df",
      "needed": [
        "libstdc++.so.6",
        "libgcc_s.so.1",
@@ -244,16 +244,26 @@
      "allOriginalNamesTypesDefaultsUnchanged": true,
      "executionArchitecture": "nativeaarch64Linux",
      "baselineCommit": "26c39fb969ac925be31ba14aa60c54f0ec088dff"
+    },
+    {
+      "originalKeyCount": 279,
+      "rebuiltKeyCount": 280,
+      "exactAddedKeys": [
+        "PlanLong"
+      ],
+      "allOriginalNamesTypesDefaultsUnchanged": true,
+      "executionArchitecture": "nativeaarch64Linux",
+      "baselineCommit": "c425b51ba6732b395d06ebd787751321d0cde4ba"
    }
  ],
  "registryCompatibility": {
-    "originalKeyCount": 279,
-    "rebuiltKeyCount": 280,
+    "originalKeyCount": 280,
+    "rebuiltKeyCount": 281,
    "exactAddedKeys": [
-      "PlanLong"
+      "TurnInHold"
    ],
    "allOriginalNamesTypesDefaultsUnchanged": true,
    "executionArchitecture": "nativeaarch64Linux",
-    "baselineCommit": "c425b51ba6732b395d06ebd787751321d0cde4ba"
+    "baselineCommit": "b9d8f95ae98b7c3d3683d747583c00a9f00cb564"
  }
 }