diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 4bb3081fb6..70f5de338a 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -21,6 +21,7 @@ MIN_OKAY_WINDOW_SEC = 25.0 MIN_RECOVERY_BUFFER_SEC = 2.0 MIN_VEGO = 15.0 MIN_ABS_YAW_RATE = np.radians(1.0) +MAX_YAW_RATE_SANITY_CHECK = 1.0 # rad/s MIN_NCC = 0.95 MAX_LAG = 1.0 @@ -243,7 +244,7 @@ class LateralLagEstimator: for last_t in [self.last_lat_inactive_t, self.last_steering_pressed_t, self.last_steering_saturated_t] ) calib_valid = self.calibrator.calib_valid - sensors_valid = self.pose_valid and np.abs(self.yaw_rate) < 1.0 and self.yaw_rate_std < 1.0 + 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) < 2.0 and np.abs(la_desired - la_actual_pose) < 0.5 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 diff --git a/selfdrive/locationd/test/test_lagd.py b/selfdrive/locationd/test/test_lagd.py index 53af2d2573..bd5cfd6201 100644 --- a/selfdrive/locationd/test/test_lagd.py +++ b/selfdrive/locationd/test/test_lagd.py @@ -3,7 +3,7 @@ import numpy as np import time import pytest -from cereal import messaging +from cereal import messaging, log from openpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag, masked_normalized_cross_correlation, \ BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC from openpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams @@ -23,8 +23,8 @@ def process_messages(mocker, estimator, lag_frames, n_frames, vego=20.0, rejecti for i in range(n_frames): t = i * estimator.dt - desired_la = np.cos(t) - actual_la = np.cos(t - lag_frames * estimator.dt) + desired_la = np.cos(t) * 0.1 + actual_la = np.cos(t - lag_frames * estimator.dt) * 0.1 # if sample is masked out, set it to desired value (no lag) rejected = random.uniform(0, 1) < rejection_threshold @@ -41,7 +41,9 @@ def process_messages(mocker, estimator, lag_frames, n_frames, vego=20.0, rejecti (t, "livePose", mocker.Mock(orientationNED=ZeroMock(), velocityDevice=ZeroMock(), accelerationDevice=ZeroMock(), - angularVelocityDevice=ZeroMock(z=actual_yr))), + angularVelocityDevice=ZeroMock(z=actual_yr, valid=True), + posenetOK=True, inputsOK=True)), + (t, "liveCalibration", mocker.Mock(rpyCalib=[0, 0, 0], calStatus=log.LiveCalibrationData.Status.calibrated)), ] for t, w, m in msgs: estimator.handle_log(t, w, m)