params: Remove separate CarParams from each daemon (#30405)

* remove separate previous route carparams from each daemon and add centrally to controlsd

* extract out sigint handler

* make process replay work for torqued

* don't write param if None
old-commit-hash: 2eb487c9a5a905b502433db2e7c5e52a52224f39

common/params.cc

@@ -99,6 +99,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"CarParamsCache", CLEAR_ON_MANAGER_START},
     {"CarParamsPersistent", PERSISTENT},
+    {"CarParamsPrevRoute", PERSISTENT},
     {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"CompletedTrainingVersion", PERSISTENT},
     {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
@@ -154,7 +155,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"LastUpdateException", CLEAR_ON_MANAGER_START},
     {"LastUpdateTime", PERSISTENT},
     {"LiveParameters", PERSISTENT},
-    {"LiveTorqueCarParams", PERSISTENT},
     {"LiveTorqueParameters", PERSISTENT | DONT_LOG},
     {"LongitudinalPersonality", PERSISTENT},
     {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},

selfdrive/controls/controlsd.py

@@ -121,6 +121,11 @@ class Controls:
       safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
       self.CP.safetyConfigs = [safety_config]
 
+    # Write previous route's CarParams
+    prev_cp = self.params.get("CarParamsPersistent")
+    if prev_cp is not None:
+      self.params.put("CarParamsPrevRoute", prev_cp)
+
     # Write CarParams for radard
     cp_bytes = self.CP.to_bytes()
     self.params.put("CarParams", cp_bytes)

selfdrive/locationd/helpers.py

@@ -1,6 +1,12 @@
 import numpy as np
+import signal
+import sys
 from typing import List, Optional, Tuple, Any
 
+from cereal import log
+from openpilot.common.params import Params
+from openpilot.system.swaglog import cloudlog
+
 
 class NPQueue:
   def __init__(self, maxlen: int, rowsize: int) -> None:
@@ -48,3 +54,24 @@ class PointBuckets:
   def load_points(self, points: List[List[float]]) -> None:
     for point in points:
       self.add_point(*point)
+
+
+class ParameterEstimator:
+  """ Base class for parameter estimators """
+  def reset(self) -> None:
+    raise NotImplementedError
+
+  def handle_log(self, t: int, which: str, msg: log.Event) -> None:
+    raise NotImplementedError
+
+  def get_msg(self, valid: bool, with_points: bool) -> log.Event:
+    raise NotImplementedError
+
+
+def cache_points_onexit(param_name, estimator, sig, frame):
+  signal.signal(sig, signal.SIG_DFL)
+  cloudlog.warning(f"Caching {param_name} param")
+  params = Params()
+  msg = estimator.get_msg(valid=True, with_points=True)
+  params.put(param_name, msg.to_bytes())
+  sys.exit(0)

selfdrive/locationd/torqued.py

@@ -1,9 +1,9 @@
 #!/usr/bin/env python3
 import os
-import sys
 import signal
 import numpy as np
 from collections import deque, defaultdict
+from functools import partial
 
 import cereal.messaging as messaging
 from cereal import car, log
@@ -12,7 +12,7 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.system.swaglog import cloudlog
 from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
-from openpilot.selfdrive.locationd.helpers import PointBuckets
+from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, cache_points_onexit
 
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
@@ -52,7 +52,7 @@ class TorqueBuckets(PointBuckets):
         break
 
 
-class TorqueEstimator:
+class TorqueEstimator(ParameterEstimator):
   def __init__(self, CP, decimated=False):
     self.hist_len = int(HISTORY / DT_MDL)
     self.lag = CP.steerActuatorDelay + .2   # from controlsd
@@ -95,7 +95,7 @@ class TorqueEstimator:
 
     # try to restore cached params
     params = Params()
-    params_cache = params.get("LiveTorqueCarParams")
+    params_cache = params.get("CarParamsPrevRoute")
     torque_cache = params.get("LiveTorqueParameters")
     if params_cache is not None and torque_cache is not None:
       try:
@@ -116,7 +116,6 @@ class TorqueEstimator:
           cloudlog.info("restored torque params from cache")
       except Exception:
         cloudlog.exception("failed to restore cached torque params")
-        params.remove("LiveTorqueCarParams")
         params.remove("LiveTorqueParameters")
 
     self.filtered_params = {}
@@ -228,19 +227,8 @@ def main():
   with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP:
     estimator = TorqueEstimator(CP)
 
-  def cache_params(sig, frame):
-    signal.signal(sig, signal.SIG_DFL)
-    cloudlog.warning("caching torque params")
-
-    params = Params()
-    params.put("LiveTorqueCarParams", CP.as_builder().to_bytes())
-
-    msg = estimator.get_msg(with_points=True)
-    params.put("LiveTorqueParameters", msg.to_bytes())
-
-    sys.exit(0)
   if "REPLAY" not in os.environ:
-    signal.signal(signal.SIGINT, cache_params)
+    signal.signal(signal.SIGINT, partial(cache_points_onexit, "LiveTorqueParameters", estimator))
 
   while True:
     sm.update()

selfdrive/test/process_replay/process_replay.py

@@ -594,10 +594,13 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") ->
   assert initial_state in ["first", "last"]
   msg_index = 0 if initial_state == "first" else -1
 
-  assert len(car_params) > 0, "carParams required for initial state of liveParameters and liveTorqueCarParams"
+  assert len(car_params) > 0, "carParams required for initial state of liveParameters and CarParamsPrevRoute"
   CP = car_params[msg_index].carParams
 
-  custom_params = {}
+  custom_params = {
+    "CarParamsPrevRoute": CP.as_builder().to_bytes()
+  }
+
   if len(live_calibration) > 0:
     custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes()
   if len(live_parameters) > 0:
@@ -605,7 +608,6 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") ->
     lp_dict["carFingerprint"] = CP.carFingerprint
     custom_params["LiveParameters"] = json.dumps(lp_dict)
   if len(live_torque_parameters) > 0:
-    custom_params["LiveTorqueCarParams"] = CP.as_builder().to_bytes()
     custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes()
 
   return custom_params