take off twtich v2

selfdrive/controls/controlsd.py

@@ -163,17 +163,19 @@ CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01  # 1/m
 CURVATURE_HOLD_CONFIRM_MIN = 0.003  # 1/m (~7 deg) of wound curvature before capture
 CURVATURE_HOLD_CONFIRM_SWEPT = 0.6  # rad of heading swept this blinker cycle; past this the push is exit-shaping, not initiation
 
-# Pull-away twitch guard. modeld converts the action head's lateral-ACCELERATION output to
-# curvature with a 1/max(1, v)^2 divide, so its residual at pull-away (~0.02 m/s^2, the
-# head's own noise floor) reads as curvature 0.015 — a 38 deg steering command — where the
-# same 0.02 m/s^2 at highway speed is 0.2 deg. Route 78511c37 twitched on 10 of 10 straight
-# takeoffs, torque 0.57-0.76 at 1.1-1.4 m/s. The model's own planned path is the tell: it
-# read 0.000-0.004 there (6-108x disagreement), while across 11 real low-speed turns in the
-# same drive the action stayed within 0.82-2.56x its plan probe.
-TWITCH_GUARD_MAX_SPEED = 4.0    # m/s; above this the 1/v^2 amplification is gone
-TWITCH_GUARD_FADE_SPEED = 3.0   # m/s; full strength below, faded out by MAX_SPEED
-TWITCH_GUARD_PLAN_RATIO = 3.0   # allowed |action| / |plan curvature| (worst real turn: 2.56)
-TWITCH_GUARD_FLOOR = 0.002      # 1/m (~5 deg of wheel); a near-zero probe must not clamp to nothing
+# Pull-away twitch guard. modeld divides the action head's lateral-ACCELERATION output by
+# max(1, v)^2, so its residual at pull-away (~0.02 m/s^2, the head's noise floor) reads as
+# curvature 0.015 — 38 deg of wheel — where the same value at highway speed is 0.2 deg.
+# Route 78511c37 twitched on 10 of 10 straight takeoffs. The model's own planned path is the
+# tell: it read straight there while the action demanded 6-108x more.
+TWITCH_GUARD_MAX_SPEED = 4.0      # m/s; above this the 1/v^2 amplification is gone
+TWITCH_GUARD_FADE_SPEED = 3.0     # m/s; full strength below, faded out by MAX_SPEED
+TWITCH_GUARD_PLAN_RATIO = 4.0     # allowed |action| / |plan curvature|
+TWITCH_GUARD_FLOOR = 0.002        # 1/m (~5 deg); a near-zero probe must not clamp to nothing
+TWITCH_GUARD_STRAIGHT_LO = 0.005  # 1/m; a plain ratio is too permissive near straight (3x of
+TWITCH_GUARD_STRAIGHT_HI = 0.014  # 0.003 still licenses 22 deg), so fade the allowance out too
+TWITCH_GUARD_MIN_REACH = 12.0     # m; shorter plans read straight while the action legitimately
+                                  # unwinds a turn (ce2b186c51 seg 28 t=14.6). Twitches: p5 24 m
 
 
 def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
@@ -233,10 +235,17 @@ def get_plan_reach(model_v2) -> float:
 
 
 def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float:
-  # See TWITCH_GUARD_*. Magnitude only: the command is bounded, never reversed.
+  # See TWITCH_GUARD_*. Magnitude only: the command is bounded, never reversed. FAR fit alone —
+  # the near probe swings with the car's heading error, so once a twitch has yawed the car it
+  # bends to correct it and licenses the very command that caused it (seg 10 t=53.1).
   if v_ego >= TWITCH_GUARD_MAX_SPEED or curvature == 0.0:
     return curvature
-  limit = max(TWITCH_GUARD_PLAN_RATIO * abs(get_plan_spatial_curvature(model_v2)), TWITCH_GUARD_FLOOR)
+  if get_plan_reach(model_v2) < TWITCH_GUARD_MIN_REACH:
+    return curvature
+  plan = abs(_plan_circle_curvature(model_v2.position.x, model_v2.position.y,
+                                    CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR))
+  straightness = (plan - TWITCH_GUARD_STRAIGHT_LO) / (TWITCH_GUARD_STRAIGHT_HI - TWITCH_GUARD_STRAIGHT_LO)
+  limit = max(TWITCH_GUARD_PLAN_RATIO * plan * min(max(straightness, 0.0), 1.0), TWITCH_GUARD_FLOOR)
   if abs(curvature) <= limit:
     return curvature
   fade = (TWITCH_GUARD_MAX_SPEED - v_ego) / (TWITCH_GUARD_MAX_SPEED - TWITCH_GUARD_FADE_SPEED)

selfdrive/controls/tests/test_turn_lead.py

@@ -17,13 +17,16 @@ def _plan(xs, ys):
   return types.SimpleNamespace(position=types.SimpleNamespace(x=xs, y=ys))
 
 
-STRAIGHT_PLAN = _plan([i * 0.5 for i in range(200)], [0.0] * 200)
+def _arc_plan(radius, n=200):
+  # constant-radius arc, ~1 rad of heading — long enough to clear the reach gate
+  return _plan([radius * math.sin(i / n) for i in range(n)],
+               [radius * (1.0 - math.cos(i / n)) for i in range(n)])
+
+
+STRAIGHT_PLAN = _plan([i * 0.5 for i in range(200)], [0.0] * 200)  # 100 m dead straight
 STANDSTILL_STUB_PLAN = _plan([0.0, 0.3], [0.0, 0.0])
-
-
-def _arc_plan(radius):
-  return _plan([radius * math.sin(t * 0.006) for t in range(200)],
-               [radius * (1.0 - math.cos(t * 0.006)) for t in range(200)])
+TURN_PLAN = _arc_plan(30.0)          # 0.033 1/m (~81 deg of wheel), 25 m of reach
+GENTLE_BEND_PLAN = _arc_plan(143.0)  # 0.007 1/m, barely bending
 
 
 def test_turn_lead_is_suppressed_only_during_applied_angle_control():
@@ -71,16 +74,20 @@ def test_guard_fades_out_across_the_speed_band():
 
 
 # turning authority must never be reduced: a real turn's action agrees with its own plan
-@pytest.mark.parametrize("ratio", [0.8, 1.0, 2.0, 2.6])
+@pytest.mark.parametrize("ratio", [0.8, 1.0, 2.0, 3.0])
 def test_real_turns_tracking_their_own_plan_are_untouched(ratio):
-  plan = _arc_plan(7.0)
-  action = 0.1428 * ratio
-  assert limit_curvature_to_plan(plan, action, 1.2) == pytest.approx(action)
+  action = (1.0 / 30.0) * ratio
+  assert limit_curvature_to_plan(TURN_PLAN, action, 1.2) == pytest.approx(action)
 
 
-def test_degenerate_plans_do_not_raise_and_still_bound_the_command():
-  for plan in (STANDSTILL_STUB_PLAN, _plan([], [])):
-    assert abs(limit_curvature_to_plan(plan, 0.0155, 0.4)) == pytest.approx(TWITCH_GUARD_FLOOR)
+def test_a_barely_bending_plan_does_not_license_a_large_command():
+  guarded = limit_curvature_to_plan(GENTLE_BEND_PLAN, 0.0155, 1.2)
+  assert TWITCH_GUARD_FLOOR < guarded < 0.008
+
+
+@pytest.mark.parametrize("plan", [STANDSTILL_STUB_PLAN, _plan([], [])])
+def test_guard_stands_down_when_the_plan_is_too_short_to_judge(plan):
+  assert limit_curvature_to_plan(plan, 0.0155, 0.4) == pytest.approx(0.0155)
 
 
 def test_zero_command_stays_zero():