diff --git a/cereal b/cereal index 79e6e4c26d..38133307b2 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 79e6e4c26dd12ba979a5fb872b4e68a707c32203 +Subproject commit 38133307b2e6036e76684b39878e79212e545e06 diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 163c3cfd4d..9e6536f9b5 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -31,7 +31,6 @@ INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration INPUTS_WANTED = 50 # We want a little bit more than we need for stability MAX_ALLOWED_SPREAD = np.radians(2) RPY_INIT = np.array([0.0,0.0,0.0]) -WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0]) # These values are needed to accommodate biggest modelframe PITCH_LIMITS = np.array([-0.09074112085129739, 0.14907572052989657]) @@ -68,7 +67,6 @@ class Calibrator: calibration_params = params.get("CalibrationParams") self.wide_camera = params.get_bool('WideCameraOnly') rpy_init = RPY_INIT - wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT valid_blocks = 0 if param_put and calibration_params: @@ -76,34 +74,24 @@ class Calibrator: msg = log.Event.from_bytes(calibration_params) rpy_init = np.array(msg.liveCalibration.rpyCalib) valid_blocks = msg.liveCalibration.validBlocks - wide_from_device_euler = np.array(msg.liveCalibration.wideFromDeviceEuler) except Exception: cloudlog.exception("Error reading cached CalibrationParams") - self.reset(rpy_init, valid_blocks, wide_from_device_euler) + self.reset(rpy_init, valid_blocks) self.update_status() - def reset(self, rpy_init: np.ndarray = RPY_INIT, - valid_blocks: int = 0, - wide_from_device_euler: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT, - smooth_from: Optional[np.ndarray] = None) -> None: + def reset(self, rpy_init: np.ndarray = RPY_INIT, valid_blocks: int = 0, smooth_from: Optional[np.ndarray] = None) -> None: if not np.isfinite(rpy_init).all(): self.rpy = RPY_INIT.copy() else: self.rpy = rpy_init.copy() - if not np.isfinite(wide_from_device_euler).all(): - self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy() - else: - self.wide_from_device_euler = wide_from_device_euler.copy() - if not np.isfinite(valid_blocks) or valid_blocks < 0: self.valid_blocks = 0 else: self.valid_blocks = valid_blocks self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1)) - self.wide_from_device_eulers = np.tile(wide_from_device_euler, (INPUTS_WANTED, 1)) self.idx = 0 self.block_idx = 0 @@ -127,8 +115,6 @@ class Calibrator: if valid_idxs: rpys = self.rpys[valid_idxs] self.rpy = np.mean(rpys, axis=0) - wide_from_device_eulers = self.wide_from_device_eulers[valid_idxs] - self.wide_from_device_euler = np.mean(wide_from_device_eulers, axis=0) max_rpy_calib = np.array(np.max(rpys, axis=0)) min_rpy_calib = np.array(np.min(rpys, axis=0)) self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib) @@ -160,10 +146,7 @@ class Calibrator: else: return self.rpy - def handle_cam_odom(self, trans: List[float], - rot: List[float], - wide_from_device_euler: List[float], - trans_std: List[float]) -> Optional[np.ndarray]: + def handle_cam_odom(self, trans: List[float], rot: List[float], trans_std: List[float]) -> Optional[np.ndarray]: self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) @@ -182,15 +165,7 @@ class Calibrator: new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy))) new_rpy = sanity_clip(new_rpy) - if len(wide_from_device_euler): - new_wide_from_device_euler = np.array(wide_from_device_euler) - else: - new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT - - self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + - (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE) - self.wide_from_device_eulers[self.block_idx] = (self.idx*self.wide_from_device_eulers[self.block_idx] + - (BLOCK_SIZE - self.idx) * new_wide_from_device_euler) / float(BLOCK_SIZE) + self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE) self.idx = (self.idx + 1) % BLOCK_SIZE if self.idx == 0: self.block_idx += 1 @@ -212,7 +187,6 @@ class Calibrator: liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) liveCalibration.rpyCalib = smooth_rpy.tolist() liveCalibration.rpyCalibSpread = self.calib_spread.tolist() - liveCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist() if self.not_car: liveCalibration.validBlocks = INPUTS_NEEDED @@ -249,7 +223,6 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m calibrator.handle_v_ego(sm['carState'].vEgo) new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, sm['cameraOdometry'].rot, - sm['cameraOdometry'].wideFromDeviceEuler, sm['cameraOdometry'].transStd) if DEBUG and new_rpy is not None: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index d03b897f9d..230e81d949 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -a5c77655fba5563e8128fb655f69b1bbd02198aa +f5a352666728344ca5065bb44c47c1f5650b4243