From a2c46fd989ec86507c53397902fc4500a7f4f6de Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Mon, 13 Jul 2026 20:52:46 -0500 Subject: [PATCH] Hodor --- selfdrive/controls/controlsd.py | 49 +++++++++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 354b049ba..d46431a3e 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -46,6 +46,23 @@ LANE_CHANGE_SMOOTH_RELEASE_T = 2.0 # 0.6 (≈ pace-5 rate) fully tracks the arrest demand seen in logs while staying 40% below stock. LANE_CHANGE_ARREST_JERK_FLOOR = 0.6 +# Low-speed turn-intent curvature hold. Approaching a turn with the blinker on, the +# model's time-based plan collapses as the car slows to a stop: desiredCurvature decays +# to zero, the controller actively unwinds the wheel at the intersection, and on +# pull-away it re-winds too late — the car goes wide (pauseturn rlog 2026-07-13). +# The hold ratchets up on the blinker-matching model command below the release speed +# and floors the command magnitude afterwards. Below the hard speed the floor is firm; +# between hard and release speed it leaks away so a genuine end-of-turn unwind or an +# aborted turn still completes in ~1-2 s. Retention deliberately does NOT depend on the +# blinker (the stalk auto-cancels during the stop in the log), on latActive (lateral +# goes inactive at standstill on torque cars; the wheel parks on rack friction), or on +# steeringPressed (the driver's instinctive grip during the unwind is what let the +# collapse through, and the driver physically overpowers a torque command regardless). +CURVATURE_HOLD_HARD_SPEED = 4.5 * CV.MPH_TO_MS +CURVATURE_HOLD_RELEASE_SPEED = 6.0 * CV.MPH_TO_MS +CURVATURE_HOLD_LEAK_RATE = 0.04 # 1/m per s; drains a typical 0.04 intersection hold in ~1 s +CURVATURE_HOLD_STANDSTILL_TIMEOUT = 30.0 # s stopped before the held turn intent is dropped + def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float: spoofed_speed = set_speed_ms @@ -100,6 +117,8 @@ class Controls: self.curvature = 0.0 self.desired_curvature = 0.0 self.lc_smooth_release = 0.0 + self.turn_hold_curvature = 0.0 + self.turn_hold_standstill_t = 0.0 self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -223,6 +242,36 @@ class Controls: else: new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature + # Low-speed turn-intent hold (see CURVATURE_HOLD_* above). Curvature sign convention + # 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) + if CS.vEgo >= CURVATURE_HOLD_RELEASE_SPEED: + self.turn_hold_curvature = 0.0 + self.turn_hold_standstill_t = 0.0 + else: + if CS.vEgo > CURVATURE_HOLD_HARD_SPEED: + self.turn_hold_curvature = math.copysign(max(abs(self.turn_hold_curvature) - CURVATURE_HOLD_LEAK_RATE * DT_CTRL, 0.0), + self.turn_hold_curvature) + if CS.vEgo < 0.5: + self.turn_hold_standstill_t += DT_CTRL + if self.turn_hold_standstill_t > CURVATURE_HOLD_STANDSTILL_TIMEOUT: + self.turn_hold_curvature = 0.0 + 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 + elif self.turn_hold_curvature * blinker_dir < 0.0: + # blinker flipped to the other side: turn intent changed + self.turn_hold_curvature = 0.0 + if CC.latActive and self.turn_hold_curvature != 0.0: + hold_dir = math.copysign(1.0, self.turn_hold_curvature) + if new_desired_curvature * hold_dir < abs(self.turn_hold_curvature): + new_desired_curvature = self.turn_hold_curvature + jerk_factor = 1.0 if self.starpilot_toggles.lane_change_pace < 10: set_jerk = self.starpilot_toggles.lane_change_jerk_factor