diff --git a/common/params.cc b/common/params.cc index 3f6cb7044c..9b40773475 100644 --- a/common/params.cc +++ b/common/params.cc @@ -99,6 +99,7 @@ std::unordered_map 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 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}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 437cf2d7ce..19ce407932 100755 --- a/selfdrive/controls/controlsd.py +++ b/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) diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py index f41b72c703..bde21f7879 100644 --- a/selfdrive/locationd/helpers.py +++ b/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) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 829f8db417..ff1fac2c22 100755 --- a/selfdrive/locationd/torqued.py +++ b/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() diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index a520b3d740..a26e964550 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/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