Update locationd (#20410)

* this was too extreme

* unused import

* capnp wants bools

* update refs

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

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])}

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:

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-f7af4a6523a7afa631460f5168646ca32c8fa4b3
+f88eda1b667eac0654f65281a16e7713836cb705