From 6bae55ac8ef71a79c5fdd6fdd79cb58b8bc9a11f Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Mon, 13 Jul 2026 20:52:46 -0500 Subject: [PATCH] Low Speed Turn Assist When using the blinker coming up to a turn and the car near standstill or stops the model goes blind, but before it does comma will now remember what it was trying to do and continue going that direction. Manually moving the wheel or cancelling turn signal will undo this. Turns will initiate at a lower speed. --- opendbc_repo/opendbc/car/hyundai/interface.py | 7 + selfdrive/controls/controlsd.py | 183 ++++++++++++++++++ selfdrive/controls/lib/latcontrol_torque.py | 44 +++-- 3 files changed, 221 insertions(+), 13 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index 6fcfacc64..c20be87ff 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -154,6 +154,13 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_ANGLE_STEERING.value if candidate == CAR.KIA_EV9: ret.steerAtStandstill = True + if candidate == CAR.HYUNDAI_IONIQ_6: + # Keep lateral active through stops: zeroing torque at standstill dropped the + # stop-turn hold and forced a rate-limit re-ramp from zero on every pull-away + # (turn1/turn2 rlogs 2026-07-14). Torque steering has no standstill gate in the + # panda safety or the carcontroller; the MDPS tolerating held torque at 0 speed + # is being validated on-road. + ret.steerAtStandstill = True if ret.flags & HyundaiFlags.CCNC and not ret.flags & HyundaiFlags.CANFD_LKA_STEERING: ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CCNC.value diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 354b049ba..d1b1b5ee5 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -46,6 +46,105 @@ 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 decays toward the model's sustained demand, so a +# transient model dip barely sags it while a genuine end-of-turn unwind or an aborted +# turn still drains it in a few seconds. 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 +# Release must sit ABOVE the speed where the model's action wakes up mid-turn. Left +# turns cross the intersection before arcing, so the car reaches ~3.5-4 m/s while the +# action still reads ~0 (left1/left2 rlogs 2026-07-15: a 6 mph release dropped the +# floor mid-turn and visibly unwound the wheel 50 deg before the action woke; rights +# wake at ~2.2 m/s and never showed it). Hold authority above creep speed is bounded +# by the opposite-command release and the decay band, not by this ceiling. +CURVATURE_HOLD_RELEASE_SPEED = 10.0 * CV.MPH_TO_MS +# Pre-wind is a NEAR-STANDSTILL device: winding the wheel is only free when the car +# isn't moving. On rolling slow turns a plan-sourced floor applies the turn's final +# curvature at the entry, starting the arc 4-7 m early — the "turning too much" +# corrections in the stickyright1/2 and left-crossing rlogs (2026-07-16) all trace to +# plan capture while moving. Above this speed only the model's own action can raise +# the hold, rate-limited so a single-frame action spike (left1 rlog 21.9s: -0.157 for +# one model frame) can't get captured and floored for seconds. +CURVATURE_HOLD_PLAN_SOURCE_SPEED = 2.0 * CV.MPH_TO_MS +CURVATURE_HOLD_RATCHET_RATE = 0.04 # 1/m per s, hold growth limit above the plan-source speed +# Once the model's action has sustainably taken over the turn, hand off COMPLETELY: +# clear the hold and don't re-engage until the blinker cycle ends. A floor that chases +# the awake action only distorts the model's entry spiral, mid-turn shape, and exit +# unwind — the sticky right-turn exits were the floor trailing the model's unwind by +# 0.03-0.05 even at the fast decay (stickyright1 41.15s: floor 0.064 vs action 0.014, +# driver correcting +394). The bridge job is done the moment the action is awake. +CURVATURE_HOLD_HANDOFF_FRAC = 0.75 +CURVATURE_HOLD_HANDOFF_TIME = 0.3 # s of sustained action >= frac*hold before handoff +CURVATURE_HOLD_DECAY_TAU = 2.0 # s; hold tracks a sustained lower model demand with this time constant +# At a turn exit the model unwinds through small SAME-sign commands (curvature only +# flips negative for the final counter-steer), so the opposite-release fires late and +# the tau-2 decay melts the floor slower than the model's exit ramp — the car keeps +# arcing while the driver hauls the wheel back (tightright3/4 rlogs 2026-07-15, drv +# +500). Turn progress is the discriminator between a mid-turn dip (protect the floor; +# left-turn sags happened at ~20 deg of swept heading) and an exit (drop it; the +# sticks happened past ~80 deg): once the swept heading passes the threshold, the +# decay switches to the fast tau and runs at ANY speed, including below the hard-hold +# speed. Swept resets whenever the hold disengages. +CURVATURE_HOLD_SWEPT_EXIT = 0.9 # rad of heading actually turned (~52 deg) +CURVATURE_HOLD_EXIT_DECAY_TAU = 0.5 # s +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_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 +# 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 + + +def _plan_circle_curvature(xs, ys, lookahead: float) -> 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(xs, ys): + 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_plan_spatial_curvature(model_v2) -> 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 + # 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) + if near * far <= 0.0: + return 0.0 + return near if abs(near) < abs(far) else far + def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float: spoofed_speed = set_speed_ms @@ -100,6 +199,11 @@ 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.turn_hold_swept = 0.0 + self.turn_hold_handoff_t = 0.0 + self.turn_hold_done = False self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -223,6 +327,85 @@ 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 + self.turn_hold_swept = 0.0 + self.turn_hold_handoff_t = 0.0 + self.turn_hold_done = False + else: + if self.turn_hold_curvature == 0.0: + self.turn_hold_swept = 0.0 + else: + # heading actually swept in the hold's direction: the measure of turn progress + self.turn_hold_swept += max(CS.vEgo * self.curvature * math.copysign(1.0, self.turn_hold_curvature), 0.0) * DT_CTRL + turn_exiting = self.turn_hold_swept > CURVATURE_HOLD_SWEPT_EXIT + if (CS.vEgo > CURVATURE_HOLD_HARD_SPEED or turn_exiting) and CC.latActive and self.turn_hold_curvature != 0.0: + # Decay toward the model's sustained same-direction demand instead of leaking on + # wall-clock time: a wall-clock leak drained the floor mid-turn while the model + # dipped transiently (turnn rlog 40.0-40.5s), while sustained low demand (end of + # turn, abort) still drains the hold within a couple of time constants. + hold_dir = math.copysign(1.0, self.turn_hold_curvature) + model_mag = max(new_desired_curvature * hold_dir, 0.0) + if model_mag < abs(self.turn_hold_curvature): + decay_tau = CURVATURE_HOLD_EXIT_DECAY_TAU if turn_exiting else CURVATURE_HOLD_DECAY_TAU + decayed = abs(self.turn_hold_curvature) + (model_mag - abs(self.turn_hold_curvature)) * (DT_CTRL / decay_tau) + self.turn_hold_curvature = math.copysign(decayed, 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 + # a stop resets the turn cycle: the model goes blind again, so a prior handoff + # must not block the standstill pre-wind (turn4 regression in v9 replay) + self.turn_hold_done = False + else: + self.turn_hold_standstill_t = 0.0 + if CC.latActive and self.turn_hold_curvature != 0.0 and \ + new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) < -CURVATURE_HOLD_OPPOSITE_RELEASE: + # model is actively counter-steering: the turn is over, release at any speed + self.turn_hold_curvature = 0.0 + self.turn_hold_done = True + if CC.latActive and self.turn_hold_curvature != 0.0 and \ + new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) >= CURVATURE_HOLD_HANDOFF_FRAC * abs(self.turn_hold_curvature): + self.turn_hold_handoff_t += DT_CTRL + if self.turn_hold_handoff_t > CURVATURE_HOLD_HANDOFF_TIME: + # action has sustainably taken over: hand off completely (see HANDOFF consts) + self.turn_hold_curvature = 0.0 + self.turn_hold_done = True + else: + self.turn_hold_handoff_t = 0.0 + if blinker_dir == 0.0: + # blinker cycle over: a fresh turn may engage a fresh hold + 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 + # 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 + 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) + if plan_curvature * blinker_dir > turn_candidate * blinker_dir: + turn_candidate = plan_curvature + if turn_candidate * blinker_dir > abs(self.turn_hold_curvature): + new_mag = turn_candidate * blinker_dir + if CS.vEgo > CURVATURE_HOLD_PLAN_SOURCE_SPEED: + 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: + # 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 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 002413782..08ac0b7a6 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -46,6 +46,13 @@ UNWIND_D_DES_THRESHOLD = -1.0 UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3 MIN_LATERAL_CONTROL_SPEED = 0.3 +# Roll compensation and latAccelOffset are lateral-accel-domain corrections; below +# walking pace the desired lateral accel is ~0 so an unfaded road-crown term dominates +# the whole feedforward and actively unwinds a held wheel at pull-away (newturn rlog +# 18.3-18.7s: ff pinned at -0.5 against a correct right-turn hold at 0.3 m/s). +FF_ROLL_OFFSET_FADE_BP = [0.5, 2.5] # m/s +FF_ROLL_OFFSET_FADE_V = [0.0, 1.0] + class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): super().__init__(CP, CI, dt) @@ -55,8 +62,12 @@ class LatControlTorque(LatControl): self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1/self.dt) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) - self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len, maxlen=self.lat_accel_request_buffer_len) + self.request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) + # Stores requested CURVATURE, scaled by the current v^2 on read. Storing lateral + # accel directly makes the delayed request lag the measurement whenever speed is + # changing (both scale with v^2 but the buffered value used the old speed), which + # at creep-speed gains reads as a phantom unwind error during every pull-away. + self.curvature_request_buffer = deque([0.] * self.request_buffer_len, maxlen=self.request_buffer_len) self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt) self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) self.ioniq_6_directional_taper_filter = FirstOrderFilter(1.0, IONIQ_6_DIRECTIONAL_TAPER_FILTER_RC, self.dt) @@ -163,28 +174,36 @@ class LatControlTorque(LatControl): getattr(starpilot_toggles, "flm_active_profile_id", "")) set_flm_runtime_overrides(getattr(starpilot_toggles, "flm_active_overrides", None) if flm_profile_active else None) flm_surface_active = flm_profile_active and flm_runtime_overrides_active() + measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + measurement = measured_curvature * CS.vEgo ** 2 + future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 if not active: output_torque = 0.0 pid_log.active = False self.pid.reset() - self.previous_measurement = 0.0 + # Keep the request buffer and rate state primed with the live command (which tracks + # the measured curvature while inactive) instead of zeroing them. Re-engaging with a + # wound wheel against a zeroed buffer puts the setpoint ~lat_delay behind the + # measurement, and the low-speed gains turn that lag into a hard unwind shove + # (turnn rlog 38.75s: +0.8 torque against a held right turn on pull-away). + self.curvature_request_buffer.append(desired_curvature) + self.previous_measurement = measurement self.measurement_rate_filter.x = 0.0 - self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len, maxlen=self.lat_accel_request_buffer_len) - self.prev_desired_lateral_accel = 0.0 + self.jerk_filter.x = 0.0 + self.prev_desired_lateral_accel = future_desired_lateral_accel self.ioniq_6_directional_taper_filter.x = 1.0 else: if self.prev_steering_pressed and not CS.steeringPressed: self.pid.i *= self.steer_release_i_decay - measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) - roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY + roll_offset_fade = np.interp(CS.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V) + roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY * roll_offset_fade curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len)) - expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames] - future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - self.lat_accel_request_buffer.append(future_desired_lateral_accel) + delay_frames = int(np.clip(lat_delay / self.dt, 1, self.request_buffer_len)) + expected_lateral_accel = self.curvature_request_buffer[-delay_frames] * CS.vEgo ** 2 + self.curvature_request_buffer.append(desired_curvature) raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt) raw_lateral_jerk = np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) desired_lateral_jerk = np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) @@ -195,7 +214,6 @@ class LatControlTorque(LatControl): abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO) self.prev_desired_lateral_accel = setpoint - measurement = measured_curvature * CS.vEgo ** 2 measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt) measurement_rate = np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) self.previous_measurement = measurement @@ -209,7 +227,7 @@ class LatControlTorque(LatControl): pid_log.error = float(error_with_lsf) ff = gravity_adjusted_future_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll - ff -= self.torque_params.latAccelOffset + ff -= self.torque_params.latAccelOffset * roll_offset_fade ff_scale = 1.0 if self.use_bolt_ff_scaling: ff_scale = np.interp(ff, [-FF_SCALE_BLEND_LAT_ACCEL, 0.0, FF_SCALE_BLEND_LAT_ACCEL],