openpilot v0.9.7 release

date: 2024-06-11T01:36:39
master commit: f8cb04e4a8

selfdrive/locationd/SConscript

@@ -1,6 +1,6 @@
-Import('env', 'arch', 'common', 'cereal', 'messaging', 'rednose', 'transformations')
+Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
 
-loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread', 'dl']
+loc_libs = [messaging, common, 'pthread', 'dl']
 
 # build ekf models
 rednose_gen_dir = 'models/generated'

selfdrive/locationd/calibrationd.py

@@ -10,7 +10,7 @@ import gc
 import os
 import capnp
 import numpy as np
-from typing import List, NoReturn, Optional
+from typing import NoReturn
 
 from cereal import log
 import cereal.messaging as messaging
@@ -89,7 +89,7 @@ class Calibrator:
                   valid_blocks: int = 0,
                   wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT,
                   height_init: np.ndarray = HEIGHT_INIT,
-                  smooth_from: Optional[np.ndarray] = None) -> None:
+                  smooth_from: np.ndarray = None) -> None:
     if not np.isfinite(rpy_init).all():
       self.rpy = RPY_INIT.copy()
     else:
@@ -125,7 +125,7 @@ class Calibrator:
       self.old_rpy = smooth_from
       self.old_rpy_weight = 1.0
 
-  def get_valid_idxs(self) -> List[int]:
+  def get_valid_idxs(self) -> list[int]:
     # exclude current block_idx from validity window
     before_current = list(range(self.block_idx))
     after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks))
@@ -175,12 +175,12 @@ 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],
-                            road_transform_trans: List[float],
-                            road_transform_trans_std: List[float]) -> Optional[np.ndarray]:
+  def handle_cam_odom(self, trans: list[float],
+                            rot: list[float],
+                            wide_from_device_euler: list[float],
+                            trans_std: list[float],
+                            road_transform_trans: list[float],
+                            road_transform_trans_std: list[float]) -> np.ndarray | None:
     self.old_rpy_weight = max(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))

selfdrive/locationd/helpers.py

@@ -1,5 +1,5 @@
 import numpy as np
-from typing import List, Optional, Tuple, Any
+from typing import Any
 
 from cereal import log
 
@@ -12,7 +12,7 @@ class NPQueue:
   def __len__(self) -> int:
     return len(self.arr)
 
-  def append(self, pt: List[float]) -> None:
+  def append(self, pt: list[float]) -> None:
     if len(self.arr) < self.maxlen:
       self.arr = np.append(self.arr, [pt], axis=0)
     else:
@@ -21,7 +21,7 @@ class NPQueue:
 
 
 class PointBuckets:
-  def __init__(self, x_bounds: List[Tuple[float, float]], min_points: List[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None:
+  def __init__(self, x_bounds: list[tuple[float, float]], min_points: list[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None:
     self.x_bounds = x_bounds
     self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds}
     self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True))
@@ -41,13 +41,13 @@ class PointBuckets:
   def add_point(self, x: float, y: float, bucket_val: float) -> None:
     raise NotImplementedError
 
-  def get_points(self, num_points: Optional[int] = None) -> Any:
+  def get_points(self, num_points: int = None) -> Any:
     points = np.vstack([x.arr for x in self.buckets.values()])
     if num_points is None:
       return points
     return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
 
-  def load_points(self, points: List[List[float]]) -> None:
+  def load_points(self, points: list[list[float]]) -> None:
     for point in points:
       self.add_point(*point)
 

selfdrive/locationd/locationd.cc

@@ -308,14 +308,12 @@ void Localizer::input_fake_gps_observations(double current_time) {
 }
 
 void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
-  // ignore the message if the fix is invalid
-  bool gps_invalid_flag = (log.getFlags() % 2 == 0);
-  bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
+  bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
   bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
   bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
   bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
 
-  if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
+  if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
     //this->gps_valid = false;
     this->determine_gps_mode(current_time);
     return;
@@ -331,7 +329,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
 
   VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
   VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
-  float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
+  float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
   MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
   MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
 

selfdrive/locationd/models/car_kf.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 import math
 import sys
-from typing import Any, Dict
+from typing import Any
 
 import numpy as np
 
@@ -28,7 +28,7 @@ def _slice(n):
   return s
 
 
-class States():
+class States:
   # Vehicle model params
   STIFFNESS = _slice(1)  # [-]
   STEER_RATIO = _slice(1)  # [-]
@@ -70,7 +70,7 @@ class CarKalman(KalmanFilter):
   ])
   P_initial = Q.copy()
 
-  obs_noise: Dict[int, Any] = {
+  obs_noise: dict[int, Any] = {
     ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
     ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
     ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),

selfdrive/locationd/models/live_kf.py

@@ -20,7 +20,7 @@ def numpy2eigenstring(arr):
   return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
 
 
-class States():
+class States:
   ECEF_POS = slice(0, 3)  # x, y and z in ECEF in meters
   ECEF_ORIENTATION = slice(3, 7)  # quat for pose of phone in ecef
   ECEF_VELOCITY = slice(7, 10)  # ecef velocity in m/s
@@ -39,7 +39,7 @@ class States():
   ACC_BIAS_ERR = slice(18, 21)
 
 
-class LiveKalman():
+class LiveKalman:
   name = 'live'
 
   initial_x = np.array([3.88e6, -3.37e6, 3.76e6,

selfdrive/locationd/test/.gitignore

@@ -0,0 +1 @@
+out/

selfdrive/locationd/test/test_calibrationd.py

@@ -0,0 +1,112 @@
+import random
+
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal import log
+from openpilot.common.params import Params
+from openpilot.selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, \
+                                                         MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT, MAX_ALLOWED_PITCH_SPREAD, MAX_ALLOWED_YAW_SPREAD
+
+
+def process_messages(c, cam_odo_calib, cycles,
+                     cam_odo_speed=MIN_SPEED_FILTER + 1,
+                     carstate_speed=MIN_SPEED_FILTER + 1,
+                     cam_odo_yr=0.0,
+                     cam_odo_speed_std=1e-3,
+                     cam_odo_height_std=1e-3):
+  old_rpy_weight_prev = 0.0
+  for _ in range(cycles):
+    assert (old_rpy_weight_prev - c.old_rpy_weight < 1/SMOOTH_CYCLES + 1e-3)
+    old_rpy_weight_prev = c.old_rpy_weight
+    c.handle_v_ego(carstate_speed)
+    c.handle_cam_odom([cam_odo_speed,
+                       np.sin(cam_odo_calib[2]) * cam_odo_speed,
+                       -np.sin(cam_odo_calib[1]) * cam_odo_speed],
+                        [0.0, 0.0, cam_odo_yr],
+                        [0.0, 0.0, 0.0],
+                        [cam_odo_speed_std, cam_odo_speed_std, cam_odo_speed_std],
+                        [0.0, 0.0, HEIGHT_INIT.item()],
+                        [cam_odo_height_std, cam_odo_height_std, cam_odo_height_std])
+
+class TestCalibrationd:
+
+  def test_read_saved_params(self):
+    msg = messaging.new_message('liveCalibration')
+    msg.liveCalibration.validBlocks = random.randint(1, 10)
+    msg.liveCalibration.rpyCalib = [random.random() for _ in range(3)]
+    msg.liveCalibration.height = [random.random() for _ in range(1)]
+    Params().put("CalibrationParams", msg.to_bytes())
+    c = Calibrator(param_put=True)
+
+    np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy)
+    np.testing.assert_allclose(msg.liveCalibration.height, c.height)
+    assert msg.liveCalibration.validBlocks == c.valid_blocks
+
+
+  def test_calibration_basics(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED)
+    assert c.valid_blocks == INPUTS_WANTED
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+    np.testing.assert_allclose(c.height, HEIGHT_INIT)
+    c.reset()
+
+
+  def test_calibration_low_speed_reject(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed=MIN_SPEED_FILTER - 1)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, carstate_speed=MIN_SPEED_FILTER - 1)
+    assert c.valid_blocks == 0
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+    np.testing.assert_allclose(c.height, HEIGHT_INIT)
+
+
+  def test_calibration_yaw_rate_reject(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_yr=MAX_YAW_RATE_FILTER)
+    assert c.valid_blocks == 0
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+    np.testing.assert_allclose(c.height, HEIGHT_INIT)
+
+
+  def test_calibration_speed_std_reject(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed_std=1e3)
+    assert c.valid_blocks == INPUTS_NEEDED
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+
+
+  def test_calibration_speed_std_height_reject(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_height_std=1e3)
+    assert c.valid_blocks == INPUTS_NEEDED
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+
+
+  def test_calibration_auto_reset(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
+    assert c.valid_blocks == INPUTS_NEEDED
+    np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0], atol=1e-3)
+    process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*0.9, MAX_ALLOWED_YAW_SPREAD*0.9], BLOCK_SIZE + 10)
+    assert c.valid_blocks == INPUTS_NEEDED + 1
+    assert c.cal_status == log.LiveCalibrationData.Status.calibrated
+
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
+    assert c.valid_blocks == INPUTS_NEEDED
+    np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
+    process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], BLOCK_SIZE + 10)
+    assert c.valid_blocks == 1
+    assert c.cal_status == log.LiveCalibrationData.Status.recalibrating
+    np.testing.assert_allclose(c.rpy, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], atol=1e-2)
+
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
+    assert c.valid_blocks == INPUTS_NEEDED
+    np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
+    process_messages(c, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], BLOCK_SIZE + 10)
+    assert c.valid_blocks == 1
+    assert c.cal_status == log.LiveCalibrationData.Status.recalibrating
+    np.testing.assert_allclose(c.rpy, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], atol=1e-2)

selfdrive/locationd/test/test_locationd.py

@@ -0,0 +1,89 @@
+import pytest
+import json
+import random
+import time
+import capnp
+
+import cereal.messaging as messaging
+from cereal.services import SERVICE_LIST
+from openpilot.common.params import Params
+from openpilot.common.transformations.coordinates import ecef2geodetic
+
+from openpilot.system.manager.process_config import managed_processes
+
+
+class TestLocationdProc:
+  LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
+              'accelerometer', 'gyroscope', 'magnetometer']
+
+  def setup_method(self):
+    self.pm = messaging.PubMaster(self.LLD_MSGS)
+
+    self.params = Params()
+    self.params.put_bool("UbloxAvailable", True)
+    managed_processes['locationd'].prepare()
+    managed_processes['locationd'].start()
+
+  def teardown_method(self):
+    managed_processes['locationd'].stop()
+
+  def get_msg(self, name, t):
+    try:
+      msg = messaging.new_message(name)
+    except capnp.lib.capnp.KjException:
+      msg = messaging.new_message(name, 0)
+
+    if name == "gpsLocationExternal":
+      msg.gpsLocationExternal.flags = 1
+      msg.gpsLocationExternal.hasFix = True
+      msg.gpsLocationExternal.verticalAccuracy = 1.0
+      msg.gpsLocationExternal.speedAccuracy = 1.0
+      msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
+      msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
+      msg.gpsLocationExternal.latitude = float(self.lat)
+      msg.gpsLocationExternal.longitude = float(self.lon)
+      msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
+      msg.gpsLocationExternal.altitude = float(self.alt)
+    #if name == "gnssMeasurements":
+    #  msg.gnssMeasurements.measTime = t
+    #  msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
+    #  msg.gnssMeasurements.positionECEF.std = [0,0,0]
+    #  msg.gnssMeasurements.positionECEF.valid = True
+    #  msg.gnssMeasurements.velocityECEF.value = []
+    #  msg.gnssMeasurements.velocityECEF.std = [0,0,0]
+    #  msg.gnssMeasurements.velocityECEF.valid = True
+    elif name == 'cameraOdometry':
+      msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
+    msg.logMonoTime = t
+    msg.valid = True
+    return msg
+
+  def test_params_gps(self):
+    random.seed(123489234)
+    self.params.remove('LastGPSPosition')
+
+    self.x = -2710700 + (random.random() * 1e5)
+    self.y = -4280600 + (random.random() * 1e5)
+    self.z = 3850300 + (random.random() * 1e5)
+    self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
+
+    # get fake messages at the correct frequency, listed in services.py
+    msgs = []
+    for sec in range(65):
+      for name in self.LLD_MSGS:
+        for j in range(int(SERVICE_LIST[name].frequency)):
+          msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9)))
+
+    for msg in sorted(msgs, key=lambda x: x.logMonoTime):
+      self.pm.send(msg.which(), msg)
+      if msg.which() == "cameraOdometry":
+        self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005)
+    time.sleep(1)  # wait for async params write
+
+    lastGPS = json.loads(self.params.get('LastGPSPosition'))
+    assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001)
+    assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001)
+    assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.001)

selfdrive/locationd/test/test_locationd_scenarios.py

@@ -0,0 +1,223 @@
+import pytest
+import numpy as np
+from collections import defaultdict
+from enum import Enum
+
+from openpilot.tools.lib.logreader import LogReader
+from openpilot.selfdrive.test.process_replay.migration import migrate_all
+from openpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name
+
+TEST_ROUTE = "ff2bd20623fcaeaa|2023-09-05--10-14-54/4"
+GPS_MESSAGES = ['gpsLocationExternal', 'gpsLocation']
+SELECT_COMPARE_FIELDS = {
+  'yaw_rate': ['angularVelocityCalibrated', 'value', 2],
+  'roll': ['orientationNED', 'value', 0],
+  'gps_flag': ['gpsOK'],
+  'inputs_flag': ['inputsOK'],
+  'sensors_flag': ['sensorsOK'],
+}
+JUNK_IDX = 100
+
+
+class Scenario(Enum):
+  BASE = 'base'
+  GPS_OFF = 'gps_off'
+  GPS_OFF_MIDWAY = 'gps_off_midway'
+  GPS_ON_MIDWAY = 'gps_on_midway'
+  GPS_TUNNEL = 'gps_tunnel'
+  GYRO_OFF = 'gyro_off'
+  GYRO_SPIKE_MIDWAY = 'gyro_spike_midway'
+  ACCEL_OFF = 'accel_off'
+  ACCEL_SPIKE_MIDWAY = 'accel_spike_midway'
+
+
+def get_select_fields_data(logs):
+  def get_nested_keys(msg, keys):
+    val = None
+    for key in keys:
+      val = getattr(msg if val is None else val, key) if isinstance(key, str) else val[key]
+    return val
+  llk = [x.liveLocationKalman for x in logs if x.which() == 'liveLocationKalman']
+  data = defaultdict(list)
+  for msg in llk:
+    for key, fields in SELECT_COMPARE_FIELDS.items():
+      data[key].append(get_nested_keys(msg, fields))
+  for key in data:
+    data[key] = np.array(data[key][JUNK_IDX:], dtype=float)
+  return data
+
+
+def run_scenarios(scenario, logs):
+  if scenario == Scenario.BASE:
+    pass
+
+  elif scenario == Scenario.GPS_OFF:
+    logs = sorted([x for x in logs if x.which() not in GPS_MESSAGES], key=lambda x: x.logMonoTime)
+
+  elif scenario == Scenario.GPS_OFF_MIDWAY:
+    non_gps = [x for x in logs if x.which() not in GPS_MESSAGES]
+    gps = [x for x in logs if x.which() in GPS_MESSAGES]
+    logs = sorted(non_gps + gps[: len(gps) // 2], key=lambda x: x.logMonoTime)
+
+  elif scenario == Scenario.GPS_ON_MIDWAY:
+    non_gps = [x for x in logs if x.which() not in GPS_MESSAGES]
+    gps = [x for x in logs if x.which() in GPS_MESSAGES]
+    logs = sorted(non_gps + gps[len(gps) // 2:], key=lambda x: x.logMonoTime)
+
+  elif scenario == Scenario.GPS_TUNNEL:
+    non_gps = [x for x in logs if x.which() not in GPS_MESSAGES]
+    gps = [x for x in logs if x.which() in GPS_MESSAGES]
+    logs = sorted(non_gps + gps[:len(gps) // 4] + gps[-len(gps) // 4:], key=lambda x: x.logMonoTime)
+
+  elif scenario == Scenario.GYRO_OFF:
+    logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime)
+
+  elif scenario == Scenario.GYRO_SPIKE_MIDWAY:
+    non_gyro = [x for x in logs if x.which() not in 'gyroscope']
+    gyro = [x for x in logs if x.which() in 'gyroscope']
+    temp = gyro[len(gyro) // 2].as_builder()
+    temp.gyroscope.gyroUncalibrated.v[0] += 3.0
+    gyro[len(gyro) // 2] = temp.as_reader()
+    logs = sorted(non_gyro + gyro, key=lambda x: x.logMonoTime)
+
+  elif scenario == Scenario.ACCEL_OFF:
+    logs = sorted([x for x in logs if x.which() != 'accelerometer'], key=lambda x: x.logMonoTime)
+
+  elif scenario == Scenario.ACCEL_SPIKE_MIDWAY:
+    non_accel = [x for x in logs if x.which() not in 'accelerometer']
+    accel = [x for x in logs if x.which() in 'accelerometer']
+    temp = accel[len(accel) // 2].as_builder()
+    temp.accelerometer.acceleration.v[0] += 10.0
+    accel[len(accel) // 2] = temp.as_reader()
+    logs = sorted(non_accel + accel, key=lambda x: x.logMonoTime)
+
+  replayed_logs = replay_process_with_name(name='locationd', lr=logs)
+  return get_select_fields_data(logs), get_select_fields_data(replayed_logs)
+
+
+@pytest.mark.xdist_group("test_locationd_scenarios")
+@pytest.mark.shared_download_cache
+class TestLocationdScenarios:
+  """
+  Test locationd with different scenarios. In all these scenarios, we expect the following:
+    - locationd kalman filter should never go unstable (we care mostly about yaw_rate, roll, gpsOK, inputsOK, sensorsOK)
+    - faulty values should be ignored, with appropriate flags set
+  """
+
+  @classmethod
+  def setup_class(cls):
+    cls.logs = migrate_all(LogReader(TEST_ROUTE))
+
+  def test_base(self):
+    """
+    Test: unchanged log
+    Expected Result:
+      - yaw_rate: unchanged
+      - roll: unchanged
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs)
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))
+
+  def test_gps_off(self):
+    """
+    Test: no GPS message for the entire segment
+    Expected Result:
+      - yaw_rate: unchanged
+      - roll:
+      - gpsOK: False
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF, self.logs)
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))
+    assert np.all(replayed_data['gps_flag'] == 0.0)
+
+  def test_gps_off_midway(self):
+    """
+    Test: no GPS message for the second half of the segment
+    Expected Result:
+      - yaw_rate: unchanged
+      - roll:
+      - gpsOK: True for the first half, False for the second half
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF_MIDWAY, self.logs)
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))
+    assert np.diff(replayed_data['gps_flag'])[512] == -1.0
+
+  def test_gps_on_midway(self):
+    """
+    Test: no GPS message for the first half of the segment
+    Expected Result:
+      - yaw_rate: unchanged
+      - roll:
+      - gpsOK: False for the first half, True for the second half
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.GPS_ON_MIDWAY, self.logs)
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(1.5))
+    assert np.diff(replayed_data['gps_flag'])[505] == 1.0
+
+  def test_gps_tunnel(self):
+    """
+    Test: no GPS message for the middle section of the segment
+    Expected Result:
+      - yaw_rate: unchanged
+      - roll:
+      - gpsOK: False for the middle section, True for the rest
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.GPS_TUNNEL, self.logs)
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))
+    assert np.diff(replayed_data['gps_flag'])[213] == -1.0
+    assert np.diff(replayed_data['gps_flag'])[805] == 1.0
+
+  def test_gyro_off(self):
+    """
+    Test: no gyroscope message for the entire segment
+    Expected Result:
+      - yaw_rate: 0
+      - roll: 0
+      - sensorsOK: False
+    """
+    _, replayed_data = run_scenarios(Scenario.GYRO_OFF, self.logs)
+    assert np.allclose(replayed_data['yaw_rate'], 0.0)
+    assert np.allclose(replayed_data['roll'], 0.0)
+    assert np.all(replayed_data['sensors_flag'] == 0.0)
+
+  def test_gyro_spikes(self):
+    """
+    Test: a gyroscope spike in the middle of the segment
+    Expected Result:
+      - yaw_rate: unchanged
+      - roll: unchanged
+      - inputsOK: False for some time after the spike, True for the rest
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs)
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))
+    assert np.diff(replayed_data['inputs_flag'])[500] == -1.0
+    assert np.diff(replayed_data['inputs_flag'])[694] == 1.0
+
+  def test_accel_off(self):
+    """
+    Test: no accelerometer message for the entire segment
+    Expected Result:
+      - yaw_rate: 0
+      - roll: 0
+      - sensorsOK: False
+    """
+    _, replayed_data = run_scenarios(Scenario.ACCEL_OFF, self.logs)
+    assert np.allclose(replayed_data['yaw_rate'], 0.0)
+    assert np.allclose(replayed_data['roll'], 0.0)
+    assert np.all(replayed_data['sensors_flag'] == 0.0)
+
+  def test_accel_spikes(self):
+    """
+    ToDo:
+    Test: an accelerometer spike in the middle of the segment
+    Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes.
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs)
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))

selfdrive/locationd/torqued.py

@@ -159,8 +159,10 @@ class TorqueEstimator(ParameterEstimator):
   def handle_log(self, t, which, msg):
     if which == "carControl":
       self.raw_points["carControl_t"].append(t + self.lag)
-      self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer)
       self.raw_points["active"].append(msg.latActive)
+    elif which == "carOutput":
+      self.raw_points["carOutput_t"].append(t + self.lag)
+      self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer)
     elif which == "carState":
       self.raw_points["carState_t"].append(t + self.lag)
       self.raw_points["vego"].append(msg.vEgo)
@@ -172,7 +174,7 @@ class TorqueEstimator(ParameterEstimator):
         active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carControl_t'], self.raw_points['active']).astype(bool)
         steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carState_t'], self.raw_points['steer_override']).astype(bool)
         vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego'])
-        steer = np.interp(t, self.raw_points['carControl_t'], self.raw_points['steer_torque'])
+        steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque'])
         lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)
         if all(active) and (not any(steer_override)) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD):
           self.filtered_points.add_point(float(steer), float(lateral_acc))
@@ -218,7 +220,7 @@ def main(demo=False):
   config_realtime_process([0, 1, 2, 3], 5)
 
   pm = messaging.PubMaster(['liveTorqueParameters'])
-  sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll='liveLocationKalman')
+  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman')
 
   params = Params()
   with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP: