mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-18 15:52:09 +08:00
what am i even doing
This commit is contained in:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user