From 36b3921ec4e41799ae040644154bb45987912574 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Wed, 16 Apr 2025 20:18:41 -0700 Subject: [PATCH] time buffer for pose valid --- selfdrive/locationd/lagd.py | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 26e46c19ab..7e7481e208 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -25,7 +25,7 @@ MAX_YAW_RATE_SANITY_CHECK = 1.0 MIN_NCC = 0.95 MAX_LAG = 1.0 MAX_LAT_ACCEL = 2.0 -MAX_LAT_ACCEL_DIFF = 0.5 +MAX_LAT_ACCEL_DIFF = 0.6 def masked_normalized_cross_correlation(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, n: int): @@ -170,6 +170,7 @@ class LateralLagEstimator: self.last_lat_inactive_t = 0.0 self.last_steering_pressed_t = 0.0 self.last_steering_saturated_t = 0.0 + self.last_pose_invalid_t = 0.0 self.last_estimate_t = 0.0 self.calibrator = PoseCalibrator() @@ -231,25 +232,28 @@ class LateralLagEstimator: return self.points.num_okay >= int(self.okay_window_sec / self.dt) def update_points(self): + 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 + sensors_valid = self.pose_valid and np.abs(self.yaw_rate) < MAX_YAW_RATE_SANITY_CHECK and self.yaw_rate_std < MAX_YAW_RATE_SANITY_CHECK + la_valid = np.abs(la_actual_pose) <= self.max_lat_accel and np.abs(la_desired - la_actual_pose) <= self.max_lat_accel_diff + calib_valid = self.calibrator.calib_valid + 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 + if not sensors_valid or not la_valid: + self.last_pose_invalid_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 - has_recovered = all( # wait for recovery after !lat_active, steering_pressed, steering_saturated + has_recovered = all( # wait for recovery after !lat_active, steering_pressed, steering_saturated, !sensors/la_valid 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] + for last_t in [self.last_lat_inactive_t, self.last_steering_pressed_t, self.last_steering_saturated_t, self.last_pose_invalid_t] ) - calib_valid = self.calibrator.calib_valid - sensors_valid = self.pose_valid and np.abs(self.yaw_rate) < MAX_YAW_RATE_SANITY_CHECK and self.yaw_rate_std < MAX_YAW_RATE_SANITY_CHECK - la_valid = np.abs(la_actual_pose) <= self.max_lat_accel and np.abs(la_desired - la_actual_pose) <= self.max_lat_accel_diff okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and \ fast and turning and has_recovered and calib_valid and sensors_valid and la_valid