what am i even doing

This commit is contained in:
whoisdomi
2026-07-15 13:22:21 -05:00
parent eae5bff189
commit a78a7c5e06
+22 -2
View File
@@ -74,6 +74,17 @@ CURVATURE_HOLD_RELEASE_SPEED = 10.0 * CV.MPH_TO_MS
CURVATURE_HOLD_PLAN_SOURCE_SPEED = 6.0 * CV.MPH_TO_MS
CURVATURE_HOLD_RATCHET_RATE = 0.04 # 1/m per s, hold growth limit above the plan-source speed
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
@@ -179,6 +190,7 @@ class Controls:
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.pose_calibrator = PoseCalibrator()
self.calibrated_pose: Pose | None = None
@@ -309,8 +321,15 @@ class Controls:
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
else:
if CS.vEgo > CURVATURE_HOLD_HARD_SPEED and CC.latActive and self.turn_hold_curvature != 0.0:
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
@@ -318,7 +337,8 @@ class Controls:
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):
decayed = abs(self.turn_hold_curvature) + (model_mag - abs(self.turn_hold_curvature)) * (DT_CTRL / CURVATURE_HOLD_DECAY_TAU)
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