diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0ecda0f2e..100c2a76f 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -64,6 +64,33 @@ CURVATURE_HOLD_RELEASE_SPEED = 6.0 * CV.MPH_TO_MS CURVATURE_HOLD_DECAY_TAU = 2.0 # s; hold tracks a sustained lower model demand with this time constant CURVATURE_HOLD_STANDSTILL_TIMEOUT = 30.0 # s stopped before the held turn intent is dropped +# The model's time-domain action.desiredCurvature is blind below ~2.5 m/s (0.3 s ahead +# at creep speed is centimeters of road), but the plan's spatial geometry already shows +# the turn at standstill: turn3/turn4 rlogs 2026-07-14 read plan curvature 0.13-0.16 +# while the action output sat at 0.005, and the plan value matched the demand the +# action produced once rolling. Feeding it into the turn-hold ratchet lets the wheel +# pre-wind toward the real turn before the car moves. Scaled and capped conservatively: +# a too-high floor turns in tighter than the path (mild at creep lat accel), while a +# too-low one just reduces the head start. The plan flickers straight for ~1-2 s right +# at the standstill->motion transition; the ratchet holds through it by design. +CURVATURE_HOLD_PLAN_LOOKAHEAD = 7.0 # m along the plan for the spatial curvature probe +CURVATURE_HOLD_PLAN_SCALE = 0.85 +CURVATURE_HOLD_PLAN_CAP = 0.12 # 1/m + + +def get_plan_spatial_curvature(model_v2, lookahead: float = CURVATURE_HOLD_PLAN_LOOKAHEAD) -> float: + # curvature of the circle through the origin, tangent to the car's heading, passing + # through the plan point ~lookahead meters ahead: kappa = 2y / (x^2 + y^2) + px, py = 0.0, 0.0 + for x, y in zip(model_v2.position.x, model_v2.position.y): + px, py = x, y + if math.hypot(x, y) >= lookahead: + break + d2 = px * px + py * py + if d2 < 1.0: + return 0.0 + return 2.0 * py / d2 + def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float: spoofed_speed = set_speed_ms @@ -268,10 +295,18 @@ class Controls: else: self.turn_hold_standstill_t = 0.0 if blinker_dir != 0.0: - if CC.latActive and new_desired_curvature * blinker_dir > abs(self.turn_hold_curvature): - # ratchet up on the raw model command, never on the floored/measured value, - # so the hold can't feed itself and defeat the leak - self.turn_hold_curvature = new_desired_curvature + # 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 + # plan's spatial curvature (see get_plan_spatial_curvature) is the second, + # earlier-seeing source: it shows the turn at standstill while the action is + # still blind, letting the pre-wind start before the car moves. + turn_candidate = new_desired_curvature if CC.latActive else 0.0 + 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) + if CC.latActive and plan_curvature * blinker_dir > turn_candidate * blinker_dir: + turn_candidate = plan_curvature + if turn_candidate * blinker_dir > abs(self.turn_hold_curvature): + self.turn_hold_curvature = turn_candidate elif self.turn_hold_curvature * blinker_dir < 0.0: # blinker flipped to the other side: turn intent changed self.turn_hold_curvature = 0.0