From cf6c7cb2b8eb79ecfeee827432fb33b85a326de7 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 19 Mar 2021 17:12:34 -0700 Subject: [PATCH] Update locationd (#20410) * this was too extreme * unused import * capnp wants bools * update refs old-commit-hash: 8bddaa7607e659c1cb247b448684d716257a3158 --- selfdrive/locationd/locationd.py | 14 +++++++------- selfdrive/locationd/models/live_kf.py | 2 +- selfdrive/locationd/paramsd.py | 14 +++++++------- selfdrive/test/process_replay/ref_commit | 2 +- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index d061452f57..f7267f0cc1 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -62,7 +62,7 @@ class Localizer(): self.calib = np.zeros(3) self.device_from_calib = np.eye(3) self.calib_from_device = np.eye(3) - self.calibrated = 0 + self.calibrated = False self.H = get_H() self.posenet_invalid_count = 0 @@ -77,7 +77,7 @@ class Localizer(): self.device_fell = False @staticmethod - def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov): + def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov, calibrated): predicted_std = np.sqrt(np.diagonal(predicted_cov)) fix_ecef = predicted_state[States.ECEF_POS] @@ -130,12 +130,12 @@ class Localizer(): (fix.velocityDevice, vel_device, vel_device_std, True), (fix.accelerationDevice, predicted_state[States.ACCELERATION], predicted_std[States.ACCELERATION_ERR], True), (fix.orientationECEF, orientation_ecef, orientation_ecef_std, True), - (fix.calibratedOrientationECEF, calibrated_orientation_ecef, np.nan*np.zeros(3), True), + (fix.calibratedOrientationECEF, calibrated_orientation_ecef, np.nan*np.zeros(3), calibrated), (fix.orientationNED, orientation_ned, np.nan*np.zeros(3), True), (fix.angularVelocityDevice, predicted_state[States.ANGULAR_VELOCITY], predicted_std[States.ANGULAR_VELOCITY_ERR], True), - (fix.velocityCalibrated, vel_calib, vel_calib_std, True), - (fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std, True), - (fix.accelerationCalibrated, acc_calib, acc_calib_std, True), + (fix.velocityCalibrated, vel_calib, vel_calib_std, calibrated), + (fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std, calibrated), + (fix.accelerationCalibrated, acc_calib, acc_calib_std, calibrated), ] for field, value, std, valid in measurements: @@ -147,7 +147,7 @@ class Localizer(): return fix def liveLocationMsg(self): - fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P) + fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P, self.calibrated) # experimentally found these values, no false positives in 20k minutes of driving old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:]) std_spike = new_mean/old_mean > 4 and new_mean > 7 diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index 62c348272b..3d8a1f65d1 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -201,7 +201,7 @@ class LiveKalman(): ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]), ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]), ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]), - ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]), + ObservationKind.NO_ROT: np.diag([0.005**2, 0.005**2, 0.005**2]), ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]), ObservationKind.ECEF_VEL: np.diag([.5**2, .5**2, .5**2]), ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.diag([.2**2, .2**2, .2**2, .2**2])} diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 263f5cc142..96b3c26dc2 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -5,14 +5,12 @@ import json import numpy as np import cereal.messaging as messaging -from cereal import car, log +from cereal import car from common.params import Params, put_nonblocking from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States from selfdrive.locationd.models.constants import GENERATED_DIR from selfdrive.swaglog import cloudlog -KalmanStatus = log.LiveLocationKalman.Status - class ParamsLearner: def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset): @@ -38,9 +36,10 @@ class ParamsLearner: yaw_rate = msg.angularVelocityCalibrated.value[2] yaw_rate_std = msg.angularVelocityCalibrated.std[2] + yaw_rate_valid = msg.angularVelocityCalibrated.valid if self.active: - if msg.inputsOK and msg.posenetOK and msg.status == KalmanStatus.valid: + if msg.inputsOK and msg.posenetOK and yaw_rate_valid: self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_YAW_RATE, np.array([[[-yaw_rate]]]), @@ -89,9 +88,10 @@ def main(sm=None, pm=None): params = None try: - if params is not None and not all(( - abs(params.get('angleOffsetAverageDeg')) < 10.0, - min_sr <= params['steerRatio'] <= max_sr)): + angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0 + steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr + params_sane = angle_offset_sane and steer_ratio_sane + if params is not None and not params_sane: cloudlog.info(f"Invalid starting values found {params}") params = None except Exception as e: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7d815a9aa2..6276af3ea1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f7af4a6523a7afa631460f5168646ca32c8fa4b3 \ No newline at end of file +f88eda1b667eac0654f65281a16e7713836cb705 \ No newline at end of file