diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 0216b69767..80789b8886 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -147,13 +147,13 @@ class LocationEstimator: self.car_speed = abs(msg.vEgo) elif which == "liveCalibration": + # Note that we use this message during calibration if len(msg.rpyCalib) > 0: calib = np.array(msg.rpyCalib) if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK: return HandleLogResult.INPUT_INVALID self.device_from_calib = rot_from_euler(calib) - self.calibrated = msg.calStatus == log.LiveCalibrationData.Status.calibrated elif which == "cameraOdometry": if not self._validate_timestamp(t): diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 0ad8d42290..8b3b9b5351 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -73,7 +73,7 @@ class ParamsLearner: 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,