From d00f7841d5668efb54afd0af7b8e37615c988505 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 27 Mar 2025 13:39:29 -0700 Subject: [PATCH] Add recovery buffer --- selfdrive/locationd/lagd.py | 23 ++++++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 5542281045..c970797a48 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -14,6 +14,7 @@ BLOCK_SIZE = 100 BLOCK_NUM = 50 MOVING_WINDOW_SEC = 300.0 MIN_OKAY_WINDOW_SEC = 30.0 +MIN_RECOVERY_BUFFER_SEC = 2.0 MIN_VEGO = 15.0 MIN_ABS_YAW_RATE = np.radians(1.0) MIN_NCC = 0.95 @@ -139,11 +140,12 @@ class BlockAverage: class LagEstimator: def __init__(self, CP, dt, block_count=BLOCK_NUM, block_size=BLOCK_SIZE, - window_sec=MOVING_WINDOW_SEC, okay_window_sec=MIN_OKAY_WINDOW_SEC, + window_sec=MOVING_WINDOW_SEC, okay_window_sec=MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec=MIN_RECOVERY_BUFFER_SEC, min_vego=MIN_VEGO, min_yr=MIN_ABS_YAW_RATE, min_ncc=MIN_NCC): self.dt = dt self.window_sec = window_sec self.okay_window_sec = okay_window_sec + self.min_recovery_buffer_sec = min_recovery_buffer_sec self.initial_lag = CP.steerActuatorDelay + 0.2 self.block_size = block_size self.block_count = block_count @@ -159,6 +161,10 @@ class LagEstimator: self.v_ego = 0 self.yaw_rate = 0 + self.last_lat_inactive_t = 0 + self.last_steering_pressed_t = 0 + self.last_steering_saturated_t = 0 + self.calibrator = PoseCalibrator() self.reset(self.initial_lag, 0) @@ -189,8 +195,8 @@ class LagEstimator: self.steering_pressed = msg.steeringPressed self.v_ego = msg.vEgo elif which == "controlsState": - self.desired_curvature = msg.desiredCurvature self.steering_saturated = getattr(msg.lateralControlState, msg.lateralControlState.which()).saturated + self.desired_curvature = msg.desiredCurvature elif which == "liveCalibration": self.calibrator.feed_live_calib(msg) elif which == "livePose": @@ -203,12 +209,23 @@ class LagEstimator: return self.points.num_okay >= int(self.okay_window_sec / self.dt) def update_points(self): + if not self.lat_active: + self.last_lat_inactive_t = self.t + if self.steering_pressed: + self.last_steering_pressed_t = self.t + if self.steering_saturated: + self.last_steering_saturated_t = self.t + la_desired = self.desired_curvature * self.v_ego * self.v_ego la_actual_pose = self.yaw_rate * self.v_ego fast = self.v_ego > self.min_vego turning = np.abs(self.yaw_rate) >= self.min_yr - okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and fast and turning + has_recovered = all([ # wait for recovery after !lat_active, steering_pressed, steering_saturated + self.t - last_t >= self.min_recovery_buffer_sec + for last_t in [self.last_lat_inactive_t, self.last_steering_pressed_t, self.last_steering_saturated_t] + ]) + okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and fast and turning and has_recovered self.points.update(self.t, la_desired, la_actual_pose, okay) if not okay or not self.points_valid():