paramsd: refactor VehicleParamsLearner (#34955)

* Refactor ParamsLearner

* Make it VehicleParamsLearner

* Fix

* Use capnp serialization instead of json

* Fix speed

* Remove redundant comments

* Monitor observed_roll

* Just use init_state

* Comment

* Improve reset

* Set globals api

* Typing for return value

* Redo reset messaging

* Remove usages of math

* Fix process_replay custom_params

* Type ignores for rednose fields

* Remove import

* Reset previous values too

* Update ref_commit

* Revert it

* Bring it back

* Remove more

* Add migration for cached params

selfdrive/locationd/models/car_kf.py

@@ -160,19 +160,18 @@ class CarKalman(KalmanFilter):
 
     gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
 
-  def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None):
-    dim_state = self.initial_x.shape[0]
-    dim_state_err = self.P_initial.shape[0]
-    x_init = self.initial_x
-    x_init[States.STEER_RATIO] = steer_ratio
-    x_init[States.STIFFNESS] = stiffness_factor
-    x_init[States.ANGLE_OFFSET] = angle_offset
-
-    if P_initial is not None:
-      self.P_initial = P_initial
-    # init filter
-    self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial,
-                              dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
+  def __init__(self, generated_dir):
+    dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
+    self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
+                              dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
+
+  def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
+    self.filter.set_global("mass", mass)
+    self.filter.set_global("rotational_inertia", rotational_inertia)
+    self.filter.set_global("center_to_front", center_to_front)
+    self.filter.set_global("center_to_rear", center_to_rear)
+    self.filter.set_global("stiffness_front", stiffness_front)
+    self.filter.set_global("stiffness_rear", stiffness_rear)
 
 
 if __name__ == "__main__":

selfdrive/locationd/paramsd.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 import os
-import math
 import json
 import numpy as np
+import capnp
 
 import cereal.messaging as messaging
 from cereal import car, log
@@ -13,12 +13,11 @@ from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from openpilot.common.swaglog import cloudlog
 
-
 MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL  # Max 20 deg/s
-ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
-ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10)
-ROLL_LOWERED_MAX = math.radians(8)
-ROLL_STD_MAX = math.radians(1.5)
+ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
+ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10)
+ROLL_LOWERED_MAX = np.radians(8)
+ROLL_STD_MAX = np.radians(1.5)
 LATERAL_ACC_SENSOR_THRESHOLD = 4.0
 OFFSET_MAX = 10.0
 OFFSET_LOWERED_MAX = 8.0
@@ -26,40 +25,58 @@ MIN_ACTIVE_SPEED = 1.0
 LOW_ACTIVE_SPEED = 10.0
 
 
-class ParamsLearner:
-  def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None):
-    self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial)
+class VehicleParamsLearner:
+  def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None):
+    self.kf = CarKalman(GENERATED_DIR)
+
+    self.x_initial = CarKalman.initial_x.copy()
+    self.x_initial[States.STEER_RATIO] = steer_ratio
+    self.x_initial[States.STIFFNESS] = stiffness_factor
+    self.x_initial[States.ANGLE_OFFSET] = angle_offset
+    self.P_initial = P_initial or CarKalman.P_initial
 
-    self.kf.filter.set_global("mass", CP.mass)
-    self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
-    self.kf.filter.set_global("center_to_front", CP.centerToFront)
-    self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront)
-    self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront)
-    self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
+    self.kf.set_globals(
+      mass=CP.mass,
+      rotational_inertia=CP.rotationalInertia,
+      center_to_front=CP.centerToFront,
+      center_to_rear=CP.wheelbase - CP.centerToFront,
+      stiffness_front=CP.tireStiffnessFront,
+      stiffness_rear=CP.tireStiffnessRear
+    )
 
-    self.active = False
+    self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
 
     self.calibrator = PoseCalibrator()
 
-    self.speed = 0.0
-    self.yaw_rate = 0.0
-    self.yaw_rate_std = 0.0
-    self.roll = 0.0
-    self.steering_angle = 0.0
+    self.observed_speed = 0.0
+    self.observed_yaw_rate = 0.0
+    self.observed_roll = 0.0
+
+    self.avg_offset_valid = True
+    self.total_offset_valid = True
+    self.roll_valid = True
+
+    self.reset(None)
 
-  def handle_log(self, t, which, msg):
+  def reset(self, t: float | None):
+    self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t)
+
+    self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False
+    self.avg_angle_offset = self.angle_offset
+
+  def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
     if which == 'livePose':
       device_pose = Pose.from_live_pose(msg)
       calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
 
+      yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
       yaw_rate_valid = msg.angularVelocityDevice.valid
-      yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10  # rad/s
-      yaw_rate_valid = yaw_rate_valid and abs(self.yaw_rate) < 1  # rad/s
-      if yaw_rate_valid:
-        self.yaw_rate, self.yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
-      else:
+      yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10  # rad/s
+      yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1  # rad/s
+      if not yaw_rate_valid:
         # This is done to bound the yaw rate estimate when localizer values are invalid or calibrating
-        self.yaw_rate, self.yaw_rate_std = 0.0, np.radians(10.0)
+        yaw_rate, yaw_rate_std = 0.0, np.radians(10.0)
+      self.observed_yaw_rate = yaw_rate
 
       localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std
       localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std
@@ -72,18 +89,18 @@ class ParamsLearner:
         # This is done to bound the road roll estimate when localizer values are invalid
         roll = 0.0
         roll_std = np.radians(10.0)
-      self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
+      self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA)
 
       if self.active:
         if msg.posenetOK:
           self.kf.predict_and_observe(t,
                                       ObservationKind.ROAD_FRAME_YAW_RATE,
-                                      np.array([[-self.yaw_rate]]),
-                                      np.array([np.atleast_2d(self.yaw_rate_std**2)]))
+                                      np.array([[-self.observed_yaw_rate]]),
+                                      np.array([np.atleast_2d(yaw_rate_std**2)]))
 
           self.kf.predict_and_observe(t,
                                       ObservationKind.ROAD_ROLL,
-                                      np.array([[self.roll]]),
+                                      np.array([[self.observed_roll]]),
                                       np.array([np.atleast_2d(roll_std**2)]))
         self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
 
@@ -99,20 +116,79 @@ class ParamsLearner:
       self.calibrator.feed_live_calib(msg)
 
     elif which == 'carState':
-      self.steering_angle = msg.steeringAngleDeg
-      self.speed = msg.vEgo
+      steering_angle = msg.steeringAngleDeg
 
-      in_linear_region = abs(self.steering_angle) < 45
-      self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region
+      in_linear_region = abs(steering_angle) < 45
+      self.observed_speed = msg.vEgo
+      self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region
 
       if self.active:
-        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
-        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]]))
+        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]]))
+        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]]))
 
     if not self.active:
       # Reset time when stopped so uncertainty doesn't grow
-      self.kf.filter.set_filter_time(t)
-      self.kf.filter.reset_rewind()
+      self.kf.filter.set_filter_time(t)  # type: ignore
+      self.kf.filter.reset_rewind()      # type: ignore
+
+  def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
+    x = self.kf.x
+    P = np.sqrt(self.kf.P.diagonal())
+    if not np.all(np.isfinite(x)):
+      cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
+      self.reset(self.kf.t)
+      x = self.kf.x
+
+    self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()),
+                                self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA)
+    self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
+                        self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA)
+    self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
+    roll_std = float(P[States.ROAD_ROLL].item())
+    if self.active and self.observed_speed > LOW_ACTIVE_SPEED:
+      # Account for the opposite signs of the yaw rates
+      # At low speeds, bumping into a curb can cause the yaw rate to be very high
+      sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
+    else:
+      sensors_valid = True
+    self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
+    self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
+    self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX)
+
+    msg = messaging.new_message('liveParameters')
+
+    msg.valid = valid
+
+    liveParameters = msg.liveParameters
+    liveParameters.posenetValid = True
+    liveParameters.sensorValid = sensors_valid
+    liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
+    liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
+    liveParameters.roll = float(self.roll)
+    liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset)
+    liveParameters.angleOffsetDeg = float(self.angle_offset)
+    liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr
+    liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
+    liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid)
+    liveParameters.angleOffsetValid = bool(self.total_offset_valid)
+    liveParameters.valid = all((
+      liveParameters.angleOffsetAverageValid,
+      liveParameters.angleOffsetValid ,
+      self.roll_valid,
+      roll_std < ROLL_STD_MAX,
+      liveParameters.stiffnessFactorValid,
+      liveParameters.steerRatioValid,
+    ))
+    liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
+    liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
+    liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
+    liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
+    if debug:
+      liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
+      liveParameters.debugFilterState.value = x.tolist()
+      liveParameters.debugFilterState.std = P.tolist()
+
+    return msg
 
 
 def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float):
@@ -123,69 +199,81 @@ def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: floa
   return current_valid
 
 
-def main():
-  config_realtime_process([0, 1, 2, 3], 5)
+# TODO: Remove this function after few releases (added in 0.9.9)
+def migrate_cached_vehicle_params_if_needed(params_reader: Params):
+  last_parameters_data = params_reader.get("LiveParameters")
+  if last_parameters_data is None:
+    return
 
-  DEBUG = bool(int(os.getenv("DEBUG", "0")))
-  REPLAY = bool(int(os.getenv("REPLAY", "0")))
-
-  pm = messaging.PubMaster(['liveParameters'])
-  sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose')
+  try:
+    last_parameters_dict = json.loads(last_parameters_data)
+    last_parameters_msg = messaging.new_message('liveParameters')
+    last_parameters_msg.liveParameters.valid = True
+    last_parameters_msg.liveParameters.steerRatio = last_parameters_dict['steerRatio']
+    last_parameters_msg.liveParameters.stiffnessFactor = last_parameters_dict['stiffnessFactor']
+    last_parameters_msg.liveParameters.angleOffsetAverageDeg = last_parameters_dict['angleOffsetAverageDeg']
+    params_reader.put("LiveParameters", last_parameters_msg.to_bytes())
+  except (json.JSONDecodeError, KeyError):
+    pass
 
-  params_reader = Params()
-  # wait for stats about the car to come in from controls
-  cloudlog.info("paramsd is waiting for CarParams")
-  CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
-  cloudlog.info("paramsd got CarParams")
 
-  min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
+def retrieve_initial_vehicle_params(params_reader: Params, CP: car.CarParams, replay: bool, debug: bool):
+  last_parameters_data = params_reader.get("LiveParameters")
+  last_carparams_data = params_reader.get("CarParamsPrevRoute")
 
-  params = params_reader.get("LiveParameters")
+  steer_ratio, stiffness_factor, angle_offset_deg, p_initial = CP.steerRatio, 1.0, 0.0, None
 
+  retrieve_success = False
+  if last_parameters_data is not None and last_carparams_data is not None:
+    try:
+      with log.Event.from_bytes(last_parameters_data) as last_lp_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP:
+        lp = last_lp_msg.liveParameters
         # Check if car model matches
-  if params is not None:
-    params = json.loads(params)
-    if params.get('carFingerprint', None) != CP.carFingerprint:
-      cloudlog.info("Parameter learner found parameters for wrong car.")
-      params = None
+        if last_CP.carFingerprint != CP.carFingerprint:
+          raise Exception("Car model mismatch")
 
         # Check if starting values are sane
-  if params is not None:
-    try:
-      steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
+        min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
+        steer_ratio_sane = min_sr <= lp.steerRatio <= max_sr
         if not steer_ratio_sane:
-        cloudlog.info(f"Invalid starting values found {params}")
-        params = None
+          raise Exception(f"Invalid starting values found {lp}")
+
+        initial_filter_std = np.array(lp.debugFilterState.std)
+        if debug and len(initial_filter_std) != 0:
+          p_initial = initial_filter_std
+
+        steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
+        retrieve_success = True
     except Exception as e:
-      cloudlog.info(f"Error reading params {params}: {str(e)}")
-      params = None
-
-  # TODO: cache the params with the capnp struct
-  if params is None:
-    params = {
-      'carFingerprint': CP.carFingerprint,
-      'steerRatio': CP.steerRatio,
-      'stiffnessFactor': 1.0,
-      'angleOffsetAverageDeg': 0.0,
-    }
-    cloudlog.info("Parameter learner resetting to default values")
+      cloudlog.error(f"Failed to retrieve initial values: {e}")
 
-  if not REPLAY:
+  if not replay:
     # When driving in wet conditions the stiffness can go down, and then be too low on the next drive
     # Without a way to detect this we have to reset the stiffness every drive
-    params['stiffnessFactor'] = 1.0
+    stiffness_factor = 1.0
 
-  pInitial = None
-  if DEBUG:
-    pInitial = np.array(params['debugFilterState']['std']) if 'debugFilterState' in params else None
+  if not retrieve_success:
+    cloudlog.info("Parameter learner resetting to default values")
 
-  learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']), pInitial)
-  angle_offset_average = params['angleOffsetAverageDeg']
-  angle_offset = angle_offset_average
-  roll = 0.0
-  avg_offset_valid = True
-  total_offset_valid = True
-  roll_valid = True
+  return steer_ratio, stiffness_factor, angle_offset_deg, p_initial
+
+
+def main():
+  config_realtime_process([0, 1, 2, 3], 5)
+
+  DEBUG = bool(int(os.getenv("DEBUG", "0")))
+  REPLAY = bool(int(os.getenv("REPLAY", "0")))
+
+  pm = messaging.PubMaster(['liveParameters'])
+  sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose')
+
+  params_reader = Params()
+  CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
+
+  migrate_cached_vehicle_params_if_needed(params_reader)
+
+  steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params_reader, CP, REPLAY, DEBUG)
+  learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial)
 
   while True:
     sm.update()
@@ -196,72 +284,13 @@ def main():
           learner.handle_log(t, which, sm[which])
 
     if sm.updated['livePose']:
-      x = learner.kf.x
-      P = np.sqrt(learner.kf.P.diagonal())
-      if not all(map(math.isfinite, x)):
-        cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
-        learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
-        x = learner.kf.x
-
-      angle_offset_average = np.clip(math.degrees(x[States.ANGLE_OFFSET].item()),
-                                  angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
-      angle_offset = np.clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
-                          angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
-      roll = np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
-      roll_std = float(P[States.ROAD_ROLL].item())
-      if learner.active and learner.speed > LOW_ACTIVE_SPEED:
-        # Account for the opposite signs of the yaw rates
-        # At low speeds, bumping into a curb can cause the yaw rate to be very high
-        sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE].item() + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
-      else:
-        sensors_valid = True
-      avg_offset_valid = check_valid_with_hysteresis(avg_offset_valid, angle_offset_average, OFFSET_MAX, OFFSET_LOWERED_MAX)
-      total_offset_valid = check_valid_with_hysteresis(total_offset_valid, angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
-      roll_valid = check_valid_with_hysteresis(roll_valid, roll, ROLL_MAX, ROLL_LOWERED_MAX)
-
-      msg = messaging.new_message('liveParameters')
-
-      liveParameters = msg.liveParameters
-      liveParameters.posenetValid = True
-      liveParameters.sensorValid = sensors_valid
-      liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
-      liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
-      liveParameters.roll = float(roll)
-      liveParameters.angleOffsetAverageDeg = float(angle_offset_average)
-      liveParameters.angleOffsetDeg = float(angle_offset)
-      liveParameters.steerRatioValid = min_sr <= liveParameters.steerRatio <= max_sr
-      liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
-      liveParameters.angleOffsetAverageValid = bool(avg_offset_valid)
-      liveParameters.angleOffsetValid = bool(total_offset_valid)
-      liveParameters.valid = all((
-        liveParameters.angleOffsetAverageValid,
-        liveParameters.angleOffsetValid ,
-        roll_valid,
-        roll_std < ROLL_STD_MAX,
-        liveParameters.stiffnessFactorValid,
-        liveParameters.steerRatioValid,
-      ))
-      liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
-      liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
-      liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
-      liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
-      if DEBUG:
-        liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
-        liveParameters.debugFilterState.value = x.tolist()
-        liveParameters.debugFilterState.std = P.tolist()
-
-      msg.valid = sm.all_checks()
+      msg = learner.get_msg(sm.all_checks(), debug=DEBUG)
 
+      msg_dat = msg.to_bytes()
       if sm.frame % 1200 == 0:  # once a minute
-        params = {
-          'carFingerprint': CP.carFingerprint,
-          'steerRatio': liveParameters.steerRatio,
-          'stiffnessFactor': liveParameters.stiffnessFactor,
-          'angleOffsetAverageDeg': liveParameters.angleOffsetAverageDeg,
-        }
-        params_reader.put_nonblocking("LiveParameters", json.dumps(params))
-
-      pm.send('liveParameters', msg)
+        params_reader.put_nonblocking("LiveParameters", msg_dat)
+
+      pm.send('liveParameters', msg_dat)
 
 
 if __name__ == "__main__":

selfdrive/test/process_replay/process_replay.py

@@ -2,7 +2,6 @@
 import os
 import time
 import copy
-import json
 import heapq
 import signal
 from collections import Counter, OrderedDict
@@ -628,9 +627,7 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") ->
   if len(live_calibration) > 0:
     custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes()
   if len(live_parameters) > 0:
-    lp_dict = live_parameters[msg_index].to_dict()
-    lp_dict["carFingerprint"] = CP.carFingerprint
-    custom_params["LiveParameters"] = json.dumps(lp_dict)
+    custom_params["LiveParameters"] = live_parameters[msg_index].as_builder().to_bytes()
   if len(live_torque_parameters) > 0:
     custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes()
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-1904f49bcc97370a842aeee1f831e9ced5a6cad6
+887623a18d82088dc5ed9ecdced55eb0d3f718b1