From 3652dff77a1220b3482c99b34f1309defa5c31d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 13 Mar 2025 02:51:25 -0400 Subject: [PATCH] paramsd: allow uncalibrated values while calibrating (#34852) * Use uncalibrated values * Use calib_valid * Move valids together * Add calibration valid field * Add migration block for new field * No paramsdTemporaryError while calibrating * comment * Fix static issues * Update ref commit * Comment * Remove redundant field * check for calstatus in selfdrived * Remove comment * Update ref commit --- selfdrive/locationd/paramsd.py | 10 +++++----- selfdrive/selfdrived/selfdrived.py | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 258fc97e5a..3819fba080 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -52,14 +52,14 @@ class ParamsLearner: device_pose = Pose.from_live_pose(msg) calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) - yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrator.calib_valid + yaw_rate_valid = msg.angularVelocityDevice.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 yaw_rate_valid: self.yaw_rate, self.yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std else: # This is done to bound the yaw rate estimate when localizer values are invalid or calibrating - self.yaw_rate, self.yaw_rate_std = 0.0, np.radians(1) + self.yaw_rate, self.yaw_rate_std = 0.0, np.radians(10.0) 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 @@ -225,13 +225,13 @@ def main(): liveParameters.posenetValid = True liveParameters.sensorValid = sensors_valid liveParameters.steerRatio = float(x[States.STEER_RATIO].item()) - liveParameters.steerRatioValid = min_sr <= liveParameters.steerRatio <= max_sr liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item()) - liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0 liveParameters.roll = float(roll) liveParameters.angleOffsetAverageDeg = float(angle_offset_average) - liveParameters.angleOffsetAverageValid = bool(avg_offset_valid) liveParameters.angleOffsetDeg = float(angle_offset) + liveParameters.steerRatioValid = min_sr <= liveParameters.steerRatio <= max_sr + liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0 + liveParameters.angleOffsetAverageValid = bool(avg_offset_valid) liveParameters.angleOffsetValid = bool(total_offset_valid) liveParameters.valid = all(( liveParameters.angleOffsetAverageValid, diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 8b7db19c9a..b81882723e 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -305,7 +305,7 @@ class SelfdriveD: self.events.add(EventName.posenetInvalid) if not self.sm['livePose'].inputsOK: self.events.add(EventName.locationdTemporaryError) - if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY): + if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY): self.events.add(EventName.paramsdTemporaryError) # conservative HW alert. if the data or frequency are off, locationd will throw an error diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index cfd179fb79..86bdc46fea 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -37041a45841e83f0641ef1e87c0e567181d47172 \ No newline at end of file +98672ccf23dc08fcd08b53cae3ec305c04219fe8 \ No newline at end of file