time buffer for pose valid

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