From 80a4ace1ab5fbc0bde40bf6cee725e61f5231359 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 9 Aug 2025 17:57:49 -0400 Subject: [PATCH 1/3] ci: use upstream docker build script for cereal validation (#1136) --- .github/workflows/cereal_validation.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/cereal_validation.yaml b/.github/workflows/cereal_validation.yaml index 9833f472fe..2290481c56 100644 --- a/.github/workflows/cereal_validation.yaml +++ b/.github/workflows/cereal_validation.yaml @@ -21,8 +21,8 @@ concurrency: env: PYTHONWARNINGS: error - BASE_IMAGE: sunnypilot-base - BUILD: release/ci/docker_build_sp.sh base + BASE_IMAGE: openpilot-base + BUILD: selfdrive/test/docker_build.sh base RUN: docker run --shm-size 2G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e CI=1 -e PYTHONWARNINGS=error -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v $GITHUB_WORKSPACE/.ci_cache/scons_cache:/tmp/scons_cache -v $GITHUB_WORKSPACE/.ci_cache/comma_download_cache:/tmp/comma_download_cache -v $GITHUB_WORKSPACE/.ci_cache/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/bash -c jobs: From b4f19d486070e2ca9abeea847e965aeae0950917 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 9 Aug 2025 20:06:10 -0400 Subject: [PATCH 2/3] Revert "`LagdToggle`: refactor and only instantiate once" (#1137) Revert "`LagdToggle`: refactor and only instantiate once (#1130)" This reverts commit 6ae668e987f4a4bb9ddf001b76f2924699993b00. --- common/params_keys.h | 2 +- selfdrive/controls/controlsd.py | 12 ++-- selfdrive/locationd/lagd.py | 6 -- selfdrive/locationd/torqued.py | 8 +-- selfdrive/modeld/modeld.py | 14 ++--- .../qt/offroad/settings/models_panel.cc | 4 ++ sunnypilot/livedelay/helpers.py | 14 ----- sunnypilot/livedelay/lagd_toggle.py | 55 ++++++++++++------- sunnypilot/modeld/modeld.py | 14 ++--- sunnypilot/modeld/modeld_base.py | 12 ---- sunnypilot/modeld_v2/modeld.py | 15 ++--- .../lib/latcontrol_torque_ext_base.py | 5 +- 12 files changed, 73 insertions(+), 88 deletions(-) delete mode 100644 sunnypilot/livedelay/helpers.py delete mode 100644 sunnypilot/modeld/modeld_base.py diff --git a/common/params_keys.h b/common/params_keys.h index 4776120c0b..2e967a3394 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 6f653a788c..61ad4754a6 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,8 +21,6 @@ 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 @@ -32,16 +30,15 @@ LaneChangeDirection = log.LaneChangeDirection ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) -class Controls(ControlsExt, ModelStateBase): +class Controls(ControlsExt): 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 and base model state + # Initialize sunnypilot controlsd extension ControlsExt.__init__(self, self.CP, self.params) - ModelStateBase.__init__(self) self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP) @@ -96,9 +93,8 @@ class Controls(ControlsExt, ModelStateBase): torque_params.frictionCoefficientFiltered) self.LaC.extension.update_model_v2(self.sm['modelV2']) - - self.lat_delay = get_lat_delay(self.params, self.lat_delay, self.sm.updated["liveDelay"]) - self.LaC.extension.update_lateral_lag(self.lat_delay) + calculated_lag = self.LaC.extension.lagd_torqued_main(self.CP, self.sm['liveDelay']) + self.LaC.extension.update_lateral_lag(calculated_lag) long_plan = self.sm['longitudinalPlan'] model_v2 = self.sm['modelV2'] diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 7f1d5fd032..d7834f7f1f 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -12,7 +12,6 @@ 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 @@ -375,8 +374,6 @@ 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(): @@ -395,6 +392,3 @@ 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 ee12f2c36c..0066708319 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -10,7 +10,8 @@ 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.helpers import get_lat_delay + +from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle HISTORY = 5 # secs POINTS_PER_BUCKET = 1500 @@ -50,7 +51,7 @@ class TorqueBuckets(PointBuckets): break -class TorqueEstimator(ParameterEstimator): +class TorqueEstimator(ParameterEstimator, LagdToggle): def __init__(self, CP, decimated=False, track_all_points=False): super().__init__() self.CP = CP @@ -98,7 +99,6 @@ class TorqueEstimator(ParameterEstimator): # 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): elif which == "liveCalibration": self.calibrator.feed_live_calib(msg) elif which == "liveDelay": - self.lag = get_lat_delay(self.params, self.lag, True) + self.lag = self.lagd_torqued_main(self.CP, msg) # 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 13f4b6e123..5c2ce90598 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -31,8 +31,7 @@ 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.helpers import get_lat_delay -from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase +from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle PROCESS_NAME = "selfdrive.modeld.modeld" @@ -80,14 +79,13 @@ 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(ModelStateBase): +class ModelState: 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) @@ -251,6 +249,8 @@ 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,8 +296,8 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - model.lat_delay = get_lat_delay(params, model.lat_delay, sm.updated["liveDelay"]) + LAT_SMOOTH_SECONDS - lateral_control_params = np.array([v_ego, model.lat_delay], dtype=np.float32) + lat_delay = modeld_lagd.lagd_main(CP, sm, model) + 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) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] @@ -343,7 +343,7 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - action = get_action_from_model(model_output, prev_action, model.lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) + action = 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/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc index 0699deef0d..45a22d7e04 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc @@ -361,6 +361,10 @@ 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 deleted file mode 100644 index fe0760c1ca..0000000000 --- a/sunnypilot/livedelay/helpers.py +++ /dev/null @@ -1,14 +0,0 @@ -""" -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, cur_val: float, updated: bool) -> float: - if updated and params.get_bool("LagdToggle"): - return float(params.get("LagdValueCache", return_default=True)) - - return cur_val diff --git a/sunnypilot/livedelay/lagd_toggle.py b/sunnypilot/livedelay/lagd_toggle.py index 8495dcee0a..ee135fb877 100644 --- a/sunnypilot/livedelay/lagd_toggle.py +++ b/sunnypilot/livedelay/lagd_toggle.py @@ -4,35 +4,48 @@ 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, CP: structs.CarParams): - self.CP = CP + def __init__(self): self.params = Params() self.lag = 0.0 + self._last_desc = None - self.lagd_toggle = self.params.get_bool("LagdToggle") - self.software_delay = self.params.get("LagdToggleDelay", return_default=True) + @property + def software_delay(self): + return self.params.get("LagdToggleDelay") - def read_params(self) -> None: - 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 update(self, lag_msg: log.LiveDelayData) -> None: - self.read_params() + 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 - 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 + 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 - lateral_delay = lag_msg.liveDelay.lateralDelay - self.lag = lateral_delay - self.params.put_nonblocking("LagdValueCache", self.lag) + 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 diff --git a/sunnypilot/modeld/modeld.py b/sunnypilot/modeld/modeld.py index 8e20e6b62a..10725efb71 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -20,14 +20,13 @@ 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" @@ -49,7 +48,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(ModelStateBase): +class ModelState: frame: ModelFrame wide_frame: ModelFrame inputs: dict[str, np.ndarray] @@ -58,7 +57,6 @@ class ModelState(ModelStateBase): 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) @@ -204,6 +202,8 @@ 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,7 +248,7 @@ def main(demo=False): v_ego = sm["carState"].vEgo is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId - model.lat_delay = get_lat_delay(params, model.lat_delay, sm.updated["liveDelay"]) + model.LAT_SMOOTH_SECONDS + steer_delay = modeld_lagd.lagd_main(CP, sm, model) if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) @@ -283,7 +283,7 @@ def main(demo=False): } if "lateral_control_params" in model.inputs.keys(): - inputs['lateral_control_params'] = np.array([max(v_ego, 0.), model.lat_delay], dtype=np.float32) + inputs['lateral_control_params'] = np.array([max(v_ego, 0.), steer_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 +306,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, model.lat_delay, model.meta) + v_ego, steer_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 deleted file mode 100644 index ba57659b09..0000000000 --- a/sunnypilot/modeld/modeld_base.py +++ /dev/null @@ -1,12 +0,0 @@ -""" -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 cec54c0033..93a8a3f05e 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -22,9 +22,8 @@ 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" @@ -40,14 +39,13 @@ class FrameMeta: self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof -class ModelState(ModelStateBase): +class ModelState: 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 @@ -244,6 +242,9 @@ 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() @@ -288,7 +289,7 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - model.lat_delay = get_lat_delay(params, model.lat_delay, sm.updated["liveDelay"]) + model.LAT_SMOOTH_SECONDS + steer_delay = modeld_lagd.lagd_main(CP, sm, model) 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))] @@ -325,7 +326,7 @@ def main(demo=False): } if "lateral_control_params" in model.numpy_inputs.keys(): - inputs['lateral_control_params'] = np.array([v_ego, model.lat_delay], dtype=np.float32) + inputs['lateral_control_params'] = np.array([v_ego, steer_delay], dtype=np.float32) mt1 = time.perf_counter() model_output = model.run(bufs, transforms, inputs, prepare_only) @@ -337,7 +338,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, model.lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) + action = model.get_action_from_model(model_output, prev_action, steer_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 6a39b31723..43d652e8b7 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py @@ -10,6 +10,8 @@ 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 @@ -41,8 +43,9 @@ def get_lookahead_value(future_vals, current_val): return min_val -class LatControlTorqueExtBase: +class LatControlTorqueExtBase(LagdToggle): 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 From 1bc12f1e21793294ddb0565edb27ada932a5efc9 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 9 Aug 2025 22:50:29 -0400 Subject: [PATCH 3/3] 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