diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b5dc4e673..d59ecc3ae 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -163,6 +163,18 @@ 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 + def _plan_circle_curvature(xs, ys, lookahead: float) -> float: # curvature of the circle through the origin, tangent to the car's heading, passing @@ -220,6 +232,18 @@ def get_plan_reach(model_v2) -> float: return xs[-1] if len(xs) else 0.0 +def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float: + # See TWITCH_GUARD_*. Magnitude only: the command is bounded, never reversed. + 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 abs(curvature) <= limit: + return curvature + fade = (TWITCH_GUARD_MAX_SPEED - v_ego) / (TWITCH_GUARD_MAX_SPEED - TWITCH_GUARD_FADE_SPEED) + fade = min(max(fade, 0.0), 1.0) + return curvature + (math.copysign(limit, curvature) - curvature) * fade + + def get_control_lateral_smooth_seconds(brand: str, v_ego: float, vehicle_smooth_seconds: float) -> float: if brand != "rivian": return LAT_SMOOTH_SECONDS @@ -481,6 +505,10 @@ class Controls: # here is positive for RIGHT turns (pauseturn log: left turn at +148 deg steering # angle logs desiredCurvature -0.07), so the blinker maps right=+1, left=-1. blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker) + # Pull-away twitch guard (see TWITCH_GUARD_*). Requires no turn intent in play, so the + # pre-wind ratchet, turn lead and exit opposite-release never see a reduced command. + if CC.latActive and blinker_dir == 0.0 and self.turn_hold_curvature == 0.0: + new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo) # heading swept in the blinker's direction over the whole blinker cycle (any speed): # discriminates a turn not yet made from one being exited (see the re-arm below) if blinker_dir == 0.0: diff --git a/selfdrive/controls/tests/test_turn_lead.py b/selfdrive/controls/tests/test_turn_lead.py index 7f3d975fd..d8b50091d 100644 --- a/selfdrive/controls/tests/test_turn_lead.py +++ b/selfdrive/controls/tests/test_turn_lead.py @@ -1,13 +1,31 @@ +import math +import types + from cereal import car import pytest -from openpilot.selfdrive.controls.controlsd import get_control_lateral_smooth_seconds, turn_lead_allowed +from openpilot.selfdrive.controls.controlsd import (TWITCH_GUARD_FLOOR, TWITCH_GUARD_MAX_SPEED, + get_control_lateral_smooth_seconds, + limit_curvature_to_plan, turn_lead_allowed) LateralControlMode = car.CarControl.Actuators.LateralControlMode +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) +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)]) + + def test_turn_lead_is_suppressed_only_during_applied_angle_control(): assert not turn_lead_allowed("rivian", LateralControlMode.angle) assert turn_lead_allowed("rivian", LateralControlMode.torque) @@ -28,3 +46,42 @@ def test_non_rivian_control_smoothing_matches_starpilot(v_ego): ]) def test_rivian_control_smoothing_remains_speed_scheduled(v_ego, expected): assert get_control_lateral_smooth_seconds("rivian", v_ego, 0.4) == pytest.approx(expected) + + +@pytest.mark.parametrize("curvature", [0.0155, -0.0155]) +def test_twitch_against_a_straight_plan_is_clamped_to_the_floor(curvature): + guarded = limit_curvature_to_plan(STRAIGHT_PLAN, curvature, 1.2) + assert abs(guarded) == pytest.approx(TWITCH_GUARD_FLOOR) + assert math.copysign(1.0, guarded) == math.copysign(1.0, curvature) + + +def test_command_already_below_the_floor_is_untouched(): + assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0015, 1.2) == pytest.approx(0.0015) + + +@pytest.mark.parametrize("v_ego", [TWITCH_GUARD_MAX_SPEED, 6.0, 30.0]) +def test_guard_is_inactive_above_its_speed_band(v_ego): + assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, v_ego) == pytest.approx(0.0155) + + +def test_guard_fades_out_across_the_speed_band(): + full = limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, 1.2) + half = limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, 3.5) + assert full < half < 0.0155 + + +# 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]) +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) + + +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_zero_command_stays_zero(): + assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0, 1.2) == 0.0