From 1bc12f1e21793294ddb0565edb27ada932a5efc9 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 9 Aug 2025 22:50:29 -0400 Subject: [PATCH] Reapply "`LagdToggle`: refactor and only instantiate once" (#1137) (#1138) * Reapply "`LagdToggle`: refactor and only instantiate once" (#1137) This reverts commit b4f19d486070e2ca9abeea847e965aeae0950917. * infinite woo gone * use them hz --- common/params_keys.h | 2 +- selfdrive/controls/controlsd.py | 12 ++-- selfdrive/locationd/lagd.py | 6 ++ selfdrive/locationd/torqued.py | 8 +-- selfdrive/modeld/modeld.py | 12 ++-- .../qt/offroad/settings/models_panel.cc | 4 -- sunnypilot/livedelay/helpers.py | 14 +++++ sunnypilot/livedelay/lagd_toggle.py | 55 +++++++------------ sunnypilot/modeld/modeld.py | 17 +++--- sunnypilot/modeld/modeld_base.py | 12 ++++ sunnypilot/modeld_v2/modeld.py | 17 +++--- .../lib/latcontrol_torque_ext_base.py | 5 +- 12 files changed, 92 insertions(+), 72 deletions(-) create mode 100644 sunnypilot/livedelay/helpers.py create mode 100644 sunnypilot/modeld/modeld_base.py diff --git a/common/params_keys.h b/common/params_keys.h index 2e967a3394..4776120c0b 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -192,8 +192,8 @@ inline static std::unordered_map keys = { // model panel params {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}}, - {"LagdToggleDesc", {PERSISTENT, STRING}}, {"LagdToggleDelay", {PERSISTENT | BACKUP, FLOAT, "0.2"}}, + {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}}, // mapd {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 61ad4754a6..360c1a4730 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,6 +21,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose +from openpilot.sunnypilot.livedelay.helpers import get_lat_delay +from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase from openpilot.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt State = log.SelfdriveState.OpenpilotState @@ -30,15 +32,16 @@ LaneChangeDirection = log.LaneChangeDirection ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) -class Controls(ControlsExt): +class Controls(ControlsExt, ModelStateBase): def __init__(self) -> None: self.params = Params() cloudlog.info("controlsd is waiting for CarParams") self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams) cloudlog.info("controlsd got CarParams") - # Initialize sunnypilot controlsd extension + # Initialize sunnypilot controlsd extension and base model state ControlsExt.__init__(self, self.CP, self.params) + ModelStateBase.__init__(self) self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP) @@ -93,8 +96,9 @@ class Controls(ControlsExt): torque_params.frictionCoefficientFiltered) self.LaC.extension.update_model_v2(self.sm['modelV2']) - calculated_lag = self.LaC.extension.lagd_torqued_main(self.CP, self.sm['liveDelay']) - self.LaC.extension.update_lateral_lag(calculated_lag) + + self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay) + self.LaC.extension.update_lateral_lag(self.lat_delay) long_plan = self.sm['longitudinalPlan'] model_v2 = self.sm['modelV2'] diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index d7834f7f1f..7f1d5fd032 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -12,6 +12,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose, fft_next_good_size, parabolic_peak_interp +from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle BLOCK_SIZE = 100 BLOCK_NUM = 50 @@ -374,6 +375,8 @@ def main(): lag, valid_blocks = initial_lag_params lag_learner.reset(lag, valid_blocks) + lagd_toggle = LagdToggle(CP) + while True: sm.update() if sm.all_checks(): @@ -392,3 +395,6 @@ def main(): if sm.frame % 1200 == 0: # cache every 60 seconds params.put_nonblocking("LiveDelay", lag_msg_dat) + + if sm.frame % 60 == 0: # read from and write to params every 3 seconds + lagd_toggle.update(lag_msg) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 0066708319..130a352c5b 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -10,8 +10,7 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose - -from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle +from openpilot.sunnypilot.livedelay.helpers import get_lat_delay HISTORY = 5 # secs POINTS_PER_BUCKET = 1500 @@ -51,7 +50,7 @@ class TorqueBuckets(PointBuckets): break -class TorqueEstimator(ParameterEstimator, LagdToggle): +class TorqueEstimator(ParameterEstimator): def __init__(self, CP, decimated=False, track_all_points=False): super().__init__() self.CP = CP @@ -99,6 +98,7 @@ class TorqueEstimator(ParameterEstimator, LagdToggle): # try to restore cached params params = Params() + self.params = params params_cache = params.get("CarParamsPrevRoute") torque_cache = params.get("LiveTorqueParameters") if params_cache is not None and torque_cache is not None: @@ -180,7 +180,7 @@ class TorqueEstimator(ParameterEstimator, LagdToggle): elif which == "liveCalibration": self.calibrator.feed_live_calib(msg) elif which == "liveDelay": - self.lag = self.lagd_torqued_main(self.CP, msg) + self.lag = get_lat_delay(self.params, msg.lateralDelay) # calculate lateral accel from past steering torque elif which == "livePose": if len(self.raw_points['steer_torque']) == self.hist_len: diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 5c2ce90598..27bf4c6ebb 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -31,7 +31,8 @@ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address -from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle +from openpilot.sunnypilot.livedelay.helpers import get_lat_delay +from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase PROCESS_NAME = "selfdrive.modeld.modeld" @@ -79,13 +80,14 @@ class FrameMeta: if vipc is not None: self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof -class ModelState: +class ModelState(ModelStateBase): frames: dict[str, DrivingModelFrame] inputs: dict[str, np.ndarray] output: np.ndarray prev_desire: np.ndarray # for tracking the rising edge of the pulse def __init__(self, context: CLContext): + ModelStateBase.__init__(self) self.LAT_SMOOTH_SECONDS = LAT_SMOOTH_SECONDS with open(VISION_METADATA_PATH, 'rb') as f: vision_metadata = pickle.load(f) @@ -249,8 +251,6 @@ def main(demo=False): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("modeld got CarParams: %s", CP.brand) - modeld_lagd = LagdToggle() - # TODO this needs more thought, use .2s extra for now to estimate other delays # TODO Move smooth seconds to action function long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS @@ -296,7 +296,9 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - lat_delay = modeld_lagd.lagd_main(CP, sm, model) + if sm.frame % 60 == 0: + model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay) + lat_delay = model.lat_delay + LAT_SMOOTH_SECONDS lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32) if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc index 45a22d7e04..0699deef0d 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc @@ -361,10 +361,6 @@ void ModelsPanel::updateLabels() { QString desc = tr("Enable this for the car to learn and adapt its steering response time. " "Disable to use a fixed steering response time. Keeping this on provides the stock openpilot experience. " "The Current value is updated automatically when the vehicle is Onroad."); - QString current = QString::fromStdString(params.get("LagdToggleDesc", false)); - if (!current.isEmpty()) { - desc += "

" + tr("Current:") + " " + current + ""; - } lagd_toggle_control->setDescription(desc); delay_control->setVisible(!params.getBool("LagdToggle")); diff --git a/sunnypilot/livedelay/helpers.py b/sunnypilot/livedelay/helpers.py new file mode 100644 index 0000000000..0f7437ceea --- /dev/null +++ b/sunnypilot/livedelay/helpers.py @@ -0,0 +1,14 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from openpilot.common.params import Params + + +def get_lat_delay(params: Params, stock_lat_delay: float) -> float: + if params.get_bool("LagdToggle"): + return float(params.get("LagdValueCache", return_default=True)) + + return stock_lat_delay diff --git a/sunnypilot/livedelay/lagd_toggle.py b/sunnypilot/livedelay/lagd_toggle.py index ee135fb877..8495dcee0a 100644 --- a/sunnypilot/livedelay/lagd_toggle.py +++ b/sunnypilot/livedelay/lagd_toggle.py @@ -4,48 +4,35 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ +from cereal import log + +from opendbc.car import structs from openpilot.common.params import Params -from openpilot.common.swaglog import cloudlog class LagdToggle: - def __init__(self): + def __init__(self, CP: structs.CarParams): + self.CP = CP self.params = Params() self.lag = 0.0 - self._last_desc = None - @property - def software_delay(self): - return self.params.get("LagdToggleDelay") + self.lagd_toggle = self.params.get_bool("LagdToggle") + self.software_delay = self.params.get("LagdToggleDelay", return_default=True) - def _maybe_update_desc(self, desc): - if desc != self._last_desc: - self.params.put_nonblocking("LagdToggleDesc", desc) - self._last_desc = desc + def read_params(self) -> None: + self.lagd_toggle = self.params.get_bool("LagdToggle") + self.software_delay = self.params.get("LagdToggleDelay", return_default=True) - def lagd_main(self, CP, sm, model): - if self.params.get_bool("LagdToggle"): - lateral_delay = sm["liveDelay"].lateralDelay - lat_smooth = model.LAT_SMOOTH_SECONDS - result = lateral_delay + lat_smooth - desc = f"live steer delay learner ({lateral_delay:.3f}s) + model smoothing filter ({lat_smooth:.3f}s) = total delay ({result:.3f}s)" - self._maybe_update_desc(desc) - return result + def update(self, lag_msg: log.LiveDelayData) -> None: + self.read_params() - steer_actuator_delay = CP.steerActuatorDelay - lat_smooth = model.LAT_SMOOTH_SECONDS - delay = self.software_delay - result = (steer_actuator_delay + delay) + lat_smooth - desc = (f"Vehicle steering delay ({steer_actuator_delay:.3f}s) + software delay ({delay:.3f}s) + " + - f"model smoothing filter ({lat_smooth:.3f}s) = total delay ({result:.3f}s)") - self._maybe_update_desc(desc) - return result + if not self.lagd_toggle: + steer_actuator_delay = self.CP.steerActuatorDelay + delay = self.software_delay + self.lag = (steer_actuator_delay + delay) + self.params.put_nonblocking("LagdValueCache", self.lag) + return - def lagd_torqued_main(self, CP, msg): - if self.params.get_bool("LagdToggle"): - self.lag = msg.lateralDelay - cloudlog.debug(f"TORQUED USING LIVE DELAY: {self.lag:.3f}") - else: - self.lag = CP.steerActuatorDelay + self.software_delay - cloudlog.debug(f"TORQUED USING STEER ACTUATOR: {self.lag:.3f}") - return self.lag + lateral_delay = lag_msg.liveDelay.lateralDelay + self.lag = lateral_delay + self.params.put_nonblocking("LagdValueCache", self.lag) diff --git a/sunnypilot/modeld/modeld.py b/sunnypilot/modeld/modeld.py index 10725efb71..b94e166bc6 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -20,13 +20,14 @@ from openpilot.system import sentry from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value +from openpilot.sunnypilot.livedelay.helpers import get_lat_delay from openpilot.sunnypilot.modeld.runners import ModelRunner, Runtime from openpilot.sunnypilot.modeld.parse_model_outputs import Parser from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants -from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext +from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase PROCESS_NAME = "selfdrive.modeld.modeld_snpe" @@ -48,7 +49,7 @@ class FrameMeta: if vipc is not None: self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof -class ModelState: +class ModelState(ModelStateBase): frame: ModelFrame wide_frame: ModelFrame inputs: dict[str, np.ndarray] @@ -57,6 +58,7 @@ class ModelState: model: ModelRunner def __init__(self, context: CLContext): + ModelStateBase.__init__(self) self.frame = ModelFrame(context) self.wide_frame = ModelFrame(context) self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) @@ -202,8 +204,6 @@ def main(demo=False): cloudlog.info("modeld got CarParams: %s", CP.brand) - modeld_lagd = LagdToggle() - # Enable lagd support for sunnypilot modeld long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS prev_action = log.ModelDataV2.Action() @@ -248,8 +248,9 @@ def main(demo=False): v_ego = sm["carState"].vEgo is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId - steer_delay = modeld_lagd.lagd_main(CP, sm, model) - + if sm.frame % 60 == 0: + model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay) + lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] @@ -283,7 +284,7 @@ def main(demo=False): } if "lateral_control_params" in model.inputs.keys(): - inputs['lateral_control_params'] = np.array([max(v_ego, 0.), steer_delay], dtype=np.float32) + inputs['lateral_control_params'] = np.array([max(v_ego, 0.), lat_delay], dtype=np.float32) if "driving_style" in model.inputs.keys(): inputs['driving_style'] = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32) @@ -306,7 +307,7 @@ def main(demo=False): action = model.get_action_from_model(model_output, prev_action, long_delay + DT_MDL) fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, - v_ego, steer_delay, model.meta) + v_ego, lat_delay, model.meta) desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] diff --git a/sunnypilot/modeld/modeld_base.py b/sunnypilot/modeld/modeld_base.py new file mode 100644 index 0000000000..ba57659b09 --- /dev/null +++ b/sunnypilot/modeld/modeld_base.py @@ -0,0 +1,12 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from openpilot.common.params import Params + + +class ModelStateBase: + def __init__(self): + self.lat_delay = Params().get("LagdValueCache", return_default=True) diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index 93a8a3f05e..2dc1026c01 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -22,8 +22,9 @@ from openpilot.sunnypilot.modeld_v2.constants import Plan from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLContext from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants +from openpilot.sunnypilot.livedelay.helpers import get_lat_delay +from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase from openpilot.sunnypilot.models.helpers import get_active_bundle -from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle from openpilot.sunnypilot.models.runners.helpers import get_model_runner PROCESS_NAME = "selfdrive.modeld.modeld" @@ -39,13 +40,14 @@ class FrameMeta: self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof -class ModelState: +class ModelState(ModelStateBase): frames: dict[str, DrivingModelFrame] inputs: dict[str, np.ndarray] prev_desire: np.ndarray # for tracking the rising edge of the pulse temporal_idxs: slice | np.ndarray def __init__(self, context: CLContext): + ModelStateBase.__init__(self) try: self.model_runner = get_model_runner() self.constants = self.model_runner.constants @@ -242,9 +244,6 @@ def main(demo=False): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("modeld got CarParams: %s", CP.brand) - - modeld_lagd = LagdToggle() - # TODO Move smooth seconds to action function long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS prev_action = log.ModelDataV2.Action() @@ -289,7 +288,9 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - steer_delay = modeld_lagd.lagd_main(CP, sm, model) + if sm.frame % 60 == 0: + model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay) + lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] @@ -326,7 +327,7 @@ def main(demo=False): } if "lateral_control_params" in model.numpy_inputs.keys(): - inputs['lateral_control_params'] = np.array([v_ego, steer_delay], dtype=np.float32) + inputs['lateral_control_params'] = np.array([v_ego, lat_delay], dtype=np.float32) mt1 = time.perf_counter() model_output = model.run(bufs, transforms, inputs, prepare_only) @@ -338,7 +339,7 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - action = model.get_action_from_model(model_output, prev_action, steer_delay + DT_MDL, long_delay + DT_MDL, v_ego) + action = model.get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py index 43d652e8b7..6a39b31723 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py @@ -10,8 +10,6 @@ import numpy as np from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle - LAT_PLAN_MIN_IDX = 5 LATERAL_LAG_MOD = 0.0 # seconds, modifies how far in the future we look ahead for the lateral plan @@ -43,9 +41,8 @@ def get_lookahead_value(future_vals, current_val): return min_val -class LatControlTorqueExtBase(LagdToggle): +class LatControlTorqueExtBase: def __init__(self, lac_torque, CP, CP_SP): - LagdToggle.__init__(self) self.model_v2 = None self.model_valid = False self.torque_params = lac_torque.torque_params