From e983abb3b009f15a57ebdfbadd4f616aba5b266e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 6 Mar 2025 22:04:27 -0800 Subject: [PATCH] default values when yaw rate invalid --- selfdrive/locationd/paramsd.py | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 8b3b9b5351..1cbaa8dde5 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -46,7 +46,6 @@ class ParamsLearner: self.yaw_rate_std = 0.0 self.roll = 0.0 self.steering_angle = 0.0 - self.roll_valid = False def handle_log(self, t, which, msg): if which == 'livePose': @@ -56,8 +55,8 @@ class ParamsLearner: localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std - self.roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK - if self.roll_valid: + roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK + if roll_valid: roll = localizer_roll # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar? roll_std = 2 * localizer_roll_std @@ -67,19 +66,19 @@ class ParamsLearner: roll_std = np.radians(10.0) self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA) - yaw_rate_valid = msg.angularVelocityDevice.valid + yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrator.calib_valid yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10 # rad/s yaw_rate_valid = yaw_rate_valid and abs(self.yaw_rate) < 1 # rad/s + if not yaw_rate_valid: + self.yaw_rate = 0.0 + self.yaw_rate_std = np.radians(10.0) if self.active: if msg.posenetOK: - # Note that we update the filter with pre-calibrated yaw rates, this is to bound the yaw rate estimate - if yaw_rate_valid: - self.kf.predict_and_observe(t, - ObservationKind.ROAD_FRAME_YAW_RATE, - np.array([[-self.yaw_rate]]), - np.array([np.atleast_2d(self.yaw_rate_std**2)])) - + self.kf.predict_and_observe(t, + ObservationKind.ROAD_FRAME_YAW_RATE, + np.array([[-self.yaw_rate]]), + np.array([np.atleast_2d(self.yaw_rate_std**2)])) self.kf.predict_and_observe(t, ObservationKind.ROAD_ROLL, np.array([[self.roll]]),