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],