From 2f0864e2150b6037fc19db155d46ef2de0c104f9 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Sat, 18 Jul 2026 19:40:26 -0500 Subject: [PATCH] Low Speed Turn Assist update Slightly higher turn in speeds when possible. This follows model desires, so if the model doesn't request it or is weak not much can be done. --- selfdrive/controls/controlsd.py | 176 +++++++++++++++++++++++++++++++- 1 file changed, 171 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c66a87979..77eb6dea3 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -119,11 +119,40 @@ CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR = 4.0 # m; reads whether the turn starts NOW CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR = 7.0 # m; reads the turn's curvature CURVATURE_HOLD_PLAN_SCALE = 0.85 CURVATURE_HOLD_PLAN_CAP = 0.12 # 1/m +# Proximity gate on the pre-wind: the 4/7 m probe reads the corner's full curvature the +# instant the plan bends toward it, which at a stop-line turn is several meters before +# the car reaches the line — so it wound the wheel hard while still rolling and cut the +# inside curb (both directions), and on an early blinker it turned in before the corner +# (RL2 seg1 t=42: corner 2 m ahead, car at 1.2 m/s, pre-wind already at 0.13). The plan +# itself carries the distance: get_plan_turn_onset_dist is where the path bends. Scale +# the pre-wind from full at ONSET_NEAR to zero at ONSET_FAR_GATE so it winds only as the +# corner closes. At a real stop the onset collapses to ~0 m once stopped (model: "turn +# is here"), so the stop-line pre-wind still reaches full strength — just later, at the +# line instead of 3 m short of it. +CURVATURE_HOLD_ONSET_HEADING = 10.0 # deg of plan heading change that marks the corner +CURVATURE_HOLD_ONSET_NEAR = 1.5 # m; corner this close -> full pre-wind +CURVATURE_HOLD_ONSET_FAR_GATE = 5.0 # m; corner past this -> no pre-wind yet +CURVATURE_HOLD_ONSET_FAR = 100.0 # m; sentinel "no corner found" # The model counter-steers at every turn exit; an opposite-direction command is the # "turn is over" signal at any speed. Without this the floor converted the exit unwind # (-0.076) into a stuck +0.012 for 1.4 s and the driver had to unwind by hand # (rightturnfail rlog 33.2-34.4s). Deadband rejects the ~0.002 pull-away flickers. CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01 # 1/m +# Nudge-to-commit: creeping toward a rolling left below the release speed, the model +# often does not commit until the geometry is entered — the driver hauled the wheel +# 144-260 deg alone with the action at zero (0000087c segs 7/8, +377..+411 driver +# torque) before the model took over. Forcing the plan probe here instead is what +# caused the 2026-07-19 fights, and the driver's own torque separates those cases +# perfectly: they RESISTED the premature machine wind (-117, bracing) but PUSH when +# the gap is theirs. So the driver's matching push is the "go" signal: capture the +# curvature they have physically wound into the hold, un-rate-limited (the wheel is +# already there; holding adds no motion), so the car grabs the turn at a nudge instead +# of making them wind it all by hand. Allowed anywhere the hold exists (< release +# speed): a 3 m/s ceiling left the 0000087f seg 1 gap-taken-at-3.5-4.2 m/s haul +# (40->220 deg at +418) unassisted; the decay/handoff/opposite-release machinery +# already bounds the captured floor in that band. +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 def _plan_circle_curvature(xs, ys, lookahead: float) -> float: @@ -140,20 +169,88 @@ def _plan_circle_curvature(xs, ys, lookahead: float) -> float: return 2.0 * py / d2 -def get_plan_spatial_curvature(model_v2) -> float: +def _plan_dual_probe(model_v2, d_near: float, d_far: float) -> float: # Min-magnitude of a near and a far circle fit. The far probe alone assumes the turn # starts immediately, which over-winds wide turns whose arc begins several meters out # (wide multi-lane lefts): the near probe reads ~straight there and only grows as the - # car creeps up to the arc, so the pre-wind self-scales to the turn geometry. Sign + # car approaches the arc, so the readout self-scales to the turn geometry. Sign # disagreement means no coherent turn ahead: contribute nothing. xs, ys = model_v2.position.x, model_v2.position.y - near = _plan_circle_curvature(xs, ys, CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR) - far = _plan_circle_curvature(xs, ys, CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR) + near = _plan_circle_curvature(xs, ys, d_near) + far = _plan_circle_curvature(xs, ys, d_far) if near * far <= 0.0: return 0.0 return near if abs(near) < abs(far) else far +def get_plan_spatial_curvature(model_v2) -> float: + return _plan_dual_probe(model_v2, CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR, CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR) + + +def get_plan_turn_onset_dist(model_v2) -> float: + # Distance along the plan at which the path first bends past ONSET_HEADING from the + # car's current heading — i.e. how far ahead the corner actually starts. The pre-wind + # magnitude scales on this so a stop-line turn winds only once the corner is close, + # not the instant the plan first shows a turn several meters out (which cut the inside + # curb at a stop and turned in early on an over-eager blinker). Returns a large + # sentinel when no bend is found, so distant/straight plans read "far" and don't wind. + xs, ys = model_v2.position.x, model_v2.position.y + n = min(len(xs), len(ys)) + for i in range(2, n): + dx = xs[i] - xs[i - 1] + dy = ys[i] - ys[i - 1] + if abs(dx) < 1e-3 and abs(dy) < 1e-3: + continue + if abs(math.degrees(math.atan2(dy, dx))) > CURVATURE_HOLD_ONSET_HEADING: + return math.hypot(xs[i], ys[i]) + return CURVATURE_HOLD_ONSET_FAR + + +# Turn-initiation lead. The model's action and the fixed 4/7 m probes are anchored in +# METERS, so the seconds of warning they give shrinks with speed — at 12 mph a corner +# enters the 7 m window only ~1.3 s out, too late to wind the wheel, which is why every +# turn in the Desktop/new drive initiated only below ~5.5 m/s regardless of approach +# speed. This lead probes the plan at a constant-TIME distance instead and max-mag +# blends into the command, so initiation can start while still rolling at 9-12 mph. +# The engagement fade (authority ramps to zero as measured curvature approaches the +# lead) limits it to the initiation phase: once the car is tracking the arc, the model +# owns the turn shape — without it, the blend re-creates the 2026-07-16 rolling-turn +# front-load (stickyright2 entry: 0.084 commanded vs the model's own 0.041 spiral). A +# binary gate here limit-cycles: cutting drops demand to a still-small action, the +# wheel unwinds below the threshold, the lead re-fires — a 5 Hz demand sawtooth felt +# as wiggle (2026-07-19 drive seg 7 t=49-50). The fade instead settles at a stable +# ~2/3-of-lead equilibrium until the action takes over via the max-mag blend. +# Stop-sign approaches stay quiet because the stopping plan compresses to the stop +# line and reads ~straight until the car is nearly there (probes ~0 for 7 s of +# blinker-on coasting at 9-12 m/s). Below TURN_LEAD_MIN_SPEED the lead must stay OFF, +# not just for standstill plan flicker: at creep speed the probe's 4 m distance floor +# chord-fits a left turn's straight-then-arc entry as "arc now", demanding 3-5x the +# model's intent. The model fights back — its plan flips to correct the premature yaw, +# the wheel swings back through center, and the swing trips the stalk auto-cancel, +# killing the turn (2026-07-19 segs 6/7/8/10, all at 1.5-2.4 m/s; the same drive's +# completed turns show the model committing on its own below 3 m/s). The model's +# meter-anchored horizon gives it 2+ s of warning at creep speed — the lead's whole +# reason to exist (time-warning shrinking with speed) does not apply there. +TURN_LEAD_T = 1.3 # s of travel the probe looks ahead (~wind-up time + lat delay) +TURN_LEAD_MIN_M = 4.0 +TURN_LEAD_MAX_M = 14.0 +TURN_LEAD_MIN_SPEED = 3.0 # m/s: authority 0 here, ramps to full at FULL_SPEED +TURN_LEAD_FULL_SPEED = 4.0 # m/s +TURN_LEAD_MAX_SPEED = 7.0 # m/s (~15.7 mph) +TURN_LEAD_SCALE = 0.85 +TURN_LEAD_CAP = 0.12 # 1/m +TURN_LEAD_ENGAGED_FRAC = 0.5 # engagement fade starts here, zero authority at 1.0 +TURN_LEAD_MODEL_OPPOSE = 0.003 # 1/m: model steering this hard against the blinker vetoes the lead +# Braking-to-a-stop veto: if sustaining the current decel parks the car within this +# factor of the probe distance, the driver intends to stop short of the arc — demanding +# that arc's curvature NOW winds the wheel at a stop approach the model didn't plan +# (0000087c seg 1: lead wound 30 deg at 6 m/s while braking for a 13 s stop; driver +# yanked it back with -509 torque). Turn-approach braking releases before this trips; +# a held brake to standstill keeps it tripped, deferring the turn to the pre-wind. +TURN_LEAD_STOP_MARGIN = 1.5 +TURN_LEAD_DECEL_GATE = -0.5 # m/s^2: only project a stop when genuinely braking + + def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float: spoofed_speed = set_speed_ms @@ -212,6 +309,7 @@ class Controls: self.turn_hold_swept = 0.0 self.turn_hold_handoff_t = 0.0 self.turn_hold_done = False + self.turn_blinker_swept = 0.0 self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -339,6 +437,12 @@ 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) + # 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: + self.turn_blinker_swept = 0.0 + else: + self.turn_blinker_swept += max(CS.vEgo * self.curvature * blinker_dir, 0.0) * DT_CTRL if CS.vEgo >= CURVATURE_HOLD_RELEASE_SPEED: self.turn_hold_curvature = 0.0 self.turn_hold_standstill_t = 0.0 @@ -389,6 +493,17 @@ class Controls: if blinker_dir == 0.0: # blinker cycle over: a fresh turn may engage a fresh hold self.turn_hold_done = False + elif CC.latActive and CS.steeringPressed and CS.steeringTorque * blinker_dir < 0.0 and \ + self.curvature * blinker_dir > CURVATURE_HOLD_CONFIRM_MIN and \ + self.turn_blinker_swept < CURVATURE_HOLD_CONFIRM_SWEPT: + # an active driver push into the signaled turn BEFORE the turn is made is fresh + # turn intent: re-arm the cycle even after a prior handoff. A long blinker-on + # approach can latch done on a trivial micro-handoff and lock out + # nudge-to-commit ten seconds later at the real turn (0000087f seg 1: +418 haul + # unassisted). The swept gate keeps a light same-direction touch during the + # EXIT unwind from re-latching a large hold against the model's recentering + # (0000087f seg 4 t=14.5 in replay: floor trailed the exit by 0.07 for 1.5 s). + self.turn_hold_done = False if blinker_dir != 0.0 and not self.turn_hold_done: # Ratchet up on the raw model command, never on the floored/measured value, so # the hold can't feed itself and defeat the decay. Below the release speed the @@ -399,11 +514,30 @@ class Controls: if CC.latActive and CS.vEgo < CURVATURE_HOLD_PLAN_SOURCE_SPEED: plan_curvature = get_plan_spatial_curvature(model_v2) * CURVATURE_HOLD_PLAN_SCALE plan_curvature = max(min(plan_curvature, CURVATURE_HOLD_PLAN_CAP), -CURVATURE_HOLD_PLAN_CAP) + # Proximity gate (see CURVATURE_HOLD_ONSET_*): wind only as the corner closes, + # so a stop-line turn winds at the line and an early blinker doesn't turn in + # early. Full at ONSET_NEAR, zero at ONSET_FAR_GATE. + onset = get_plan_turn_onset_dist(model_v2) + onset_w = min(max((CURVATURE_HOLD_ONSET_FAR_GATE - onset) / + (CURVATURE_HOLD_ONSET_FAR_GATE - CURVATURE_HOLD_ONSET_NEAR), 0.0), 1.0) + plan_curvature *= onset_w if plan_curvature * blinker_dir > turn_candidate * blinker_dir: turn_candidate = plan_curvature + # Nudge-to-commit (see CURVATURE_HOLD_CONFIRM_*): the driver actively pushing in + # the blinker direction at creep speed captures what they have wound. Positive + # steeringTorque is a LEFT push (negative curvature), so agreement is a negative + # product with blinker_dir. Exempt from the ratchet rate limit: latching the + # wheel's current position commands no motion, only keeps the driver's progress. + driver_confirmed = False + if CC.latActive and CS.steeringPressed and \ + CS.steeringTorque * blinker_dir < 0.0 and self.curvature * blinker_dir > CURVATURE_HOLD_CONFIRM_MIN: + wound_curvature = max(min(self.curvature, CURVATURE_HOLD_PLAN_CAP), -CURVATURE_HOLD_PLAN_CAP) + if wound_curvature * blinker_dir > turn_candidate * blinker_dir: + turn_candidate = wound_curvature + driver_confirmed = True if turn_candidate * blinker_dir > abs(self.turn_hold_curvature): new_mag = turn_candidate * blinker_dir - if CS.vEgo > CURVATURE_HOLD_PLAN_SOURCE_SPEED: + if CS.vEgo > CURVATURE_HOLD_PLAN_SOURCE_SPEED and not driver_confirmed: new_mag = min(new_mag, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL) self.turn_hold_curvature = math.copysign(new_mag, turn_candidate) elif self.turn_hold_curvature * blinker_dir < 0.0: @@ -414,6 +548,38 @@ class Controls: if new_desired_curvature * hold_dir < abs(self.turn_hold_curvature): new_desired_curvature = self.turn_hold_curvature + # Turn-initiation lead (see TURN_LEAD_*). Applied AFTER the hold block so the + # ratchet/handoff only ever see the raw model action; pure max-magnitude, so it can + # never reduce or oppose the model. Lane changes are excluded: that blinker's plan + # bend is not a turn. The model-oppose veto is defense-in-depth for the fade-in + # edge: a model actively steering against the blinker is correcting something the + # lead must not fight (see the constants comment for the 2026-07-19 failures). + if (CC.latActive and blinker_dir != 0.0 and + model_v2.meta.laneChangeState == LaneChangeState.off and + TURN_LEAD_MIN_SPEED <= CS.vEgo < TURN_LEAD_MAX_SPEED and + new_desired_curvature * blinker_dir > -TURN_LEAD_MODEL_OPPOSE): + d_near = max(min(TURN_LEAD_T * CS.vEgo, TURN_LEAD_MAX_M), TURN_LEAD_MIN_M) + stopping_short = CS.aEgo < TURN_LEAD_DECEL_GATE and \ + CS.vEgo ** 2 / (2.0 * -CS.aEgo) < TURN_LEAD_STOP_MARGIN * d_near + lead_curvature = 0.0 if stopping_short else _plan_dual_probe(model_v2, d_near, d_near + 3.0) * TURN_LEAD_SCALE + lead_curvature = max(min(lead_curvature, TURN_LEAD_CAP), -TURN_LEAD_CAP) + if lead_curvature * blinker_dir > 0.0: + speed_w = min(max((CS.vEgo - TURN_LEAD_MIN_SPEED) / (TURN_LEAD_FULL_SPEED - TURN_LEAD_MIN_SPEED), 0.0), 1.0) + engaged_ratio = abs(self.curvature) / abs(lead_curvature) + engage_w = min(max((1.0 - engaged_ratio) / (1.0 - TURN_LEAD_ENGAGED_FRAC), 0.0), 1.0) + lead_curvature *= speed_w * engage_w + if lead_curvature * blinker_dir > max(new_desired_curvature * blinker_dir, 0.0): + new_desired_curvature = lead_curvature + # Capture the applied lead into the hold (rate-limited like any moving-speed + # ratchet) so decelerating through the fade floor keeps the initiation + # progress: without this, braking mid-wind dumped the lead's demand back to + # the still-small action and visibly unwound the wheel 50->15 deg before the + # standstill pre-wind had to redo the work (0000087c seg 6 first left). + if CS.vEgo < CURVATURE_HOLD_RELEASE_SPEED and not self.turn_hold_done and \ + lead_curvature * blinker_dir > abs(self.turn_hold_curvature): + held_mag = min(lead_curvature * blinker_dir, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL) + self.turn_hold_curvature = math.copysign(held_mag, lead_curvature) + jerk_factor = 1.0 if self.starpilot_toggles.lane_change_pace < 10: set_jerk = self.starpilot_toggles.lane_change_jerk_factor