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