From d0bd8cc4a3abe6a93fce2a605fad0b2eb0c7f16c Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Sat, 5 Jul 2025 14:42:36 -0700 Subject: [PATCH] liveDelay: Add live delay toggle to vehicles using torqued (#1001) * Add live delay toggle to torqued.py and twilsonco NNLC * Set this in init * Clean up * Live delay toggle refactor * ModeldLagd -> LagdToggle * This is for lagd_toggle.py * Add to NNLC * Lagd toggle: Display current values on UI * Add break * LagdToggleDelay Live edit software_delay when livedelay is toggled `off` * Always show description * Add description as to why values don't update offroad --------- Co-authored-by: Kumar <36933347+rav4kumar@users.noreply.github.com> --- common/params_keys.h | 2 + selfdrive/controls/controlsd.py | 3 +- selfdrive/locationd/torqued.py | 8 ++- selfdrive/modeld/modeld.py | 7 ++- .../qt/offroad/settings/models_panel.cc | 42 +++++++++++++-- .../qt/offroad/settings/models_panel.h | 2 + selfdrive/ui/sunnypilot/qt/widgets/controls.h | 25 +++++++-- sunnypilot/livedelay/__init__.py | 0 sunnypilot/livedelay/lagd_toggle.py | 51 +++++++++++++++++++ sunnypilot/modeld/modeld.py | 4 +- sunnypilot/modeld_v2/modeld.py | 4 +- sunnypilot/models/modeld_lagd.py | 26 ---------- .../lib/latcontrol_torque_ext_base.py | 5 +- system/manager/manager.py | 1 + 14 files changed, 136 insertions(+), 44 deletions(-) create mode 100644 sunnypilot/livedelay/__init__.py create mode 100644 sunnypilot/livedelay/lagd_toggle.py delete mode 100644 sunnypilot/models/modeld_lagd.py diff --git a/common/params_keys.h b/common/params_keys.h index 02dd8fee8..00e1763e1 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -178,6 +178,8 @@ inline static std::unordered_map keys = { // model panel params {"LagdToggle", PERSISTENT | BACKUP}, + {"LagdToggleDesc", PERSISTENT}, + {"LagdToggledelay", PERSISTENT | BACKUP}, // mapd {"MapAdvisorySpeedLimit", CLEAR_ON_ONROAD_TRANSITION}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 04d79d333..9ef985fcc 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -93,7 +93,8 @@ class Controls(ControlsExt): torque_params.frictionCoefficientFiltered) self.LaC.extension.update_model_v2(self.sm['modelV2']) - self.LaC.extension.update_lateral_lag(self.sm['liveDelay'].lateralDelay) + 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/torqued.py b/selfdrive/locationd/torqued.py index 23bd99931..006670831 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -11,6 +11,8 @@ 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 + HISTORY = 5 # secs POINTS_PER_BUCKET = 1500 MIN_POINTS_TOTAL = 4000 @@ -49,8 +51,10 @@ 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 self.hist_len = int(HISTORY / DT_MDL) self.lag = 0.0 self.track_all_points = track_all_points # for offline analysis, without max lateral accel or max steer torque filters @@ -176,7 +180,7 @@ class TorqueEstimator(ParameterEstimator): elif which == "liveCalibration": self.calibrator.feed_live_calib(msg) elif which == "liveDelay": - self.lag = msg.lateralDelay + 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 ac88dfbb5..3236edb53 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -37,6 +37,8 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext +from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle + PROCESS_NAME = "selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -86,6 +88,7 @@ class ModelState: prev_desire: np.ndarray # for tracking the rising edge of the pulse def __init__(self, context: CLContext): + self.LAT_SMOOTH_SECONDS = 0.0 with open(VISION_METADATA_PATH, 'rb') as f: vision_metadata = pickle.load(f) self.vision_input_shapes = vision_metadata['input_shapes'] @@ -251,6 +254,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,7 +301,7 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS + 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) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc index b8237c362..1a2e86622 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc @@ -90,11 +90,25 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) { list->addItem(horizontal_line()); // LiveDelay toggle - list->addItem(new ParamControlSP("LagdToggle", - tr("Live Learning Steer Delay"), - 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."), - "../assets/offroad/icon_shell.png")); + lagd_toggle_control = new ParamControlSP("LagdToggle", tr("Live Learning Steer Delay"), "", "../assets/offroad/icon_shell.png"); + lagd_toggle_control->showDescription(); + list->addItem(lagd_toggle_control); + + // Software delay control + delay_control = new OptionControlSP("LagdToggledelay", tr("Adjust Software Delay"), + tr("Adjust the software delay when Live Learning Steer Delay is toggled off." + "\nThe default software delay value is 0.2"), + "", {10, 30}, 1, false, nullptr, true); + + connect(delay_control, &OptionControlSP::updateLabels, [=]() { + float value = QString::fromStdString(params.get("LagdToggledelay")).toFloat(); + delay_control->setLabel(QString::number(value, 'f', 2) + "s"); + }); + connect(lagd_toggle_control, &ParamControlSP::toggleFlipped, [=](bool state) { + delay_control->setVisible(!state); + }); + delay_control->showDescription(); + list->addItem(delay_control); } QProgressBar* ModelsPanel::createProgressBar(QWidget *parent) { @@ -290,6 +304,24 @@ void ModelsPanel::updateLabels() { handleBundleDownloadProgress(); currentModelLblBtn->setEnabled(!is_onroad && !isDownloading()); currentModelLblBtn->setValue(GetActiveModelInternalName()); + + // Update lagdToggle description with current value + 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); + lagd_toggle_control->showDescription(); + + delay_control->setVisible(!params.getBool("LagdToggle")); + if (delay_control->isVisible()) { + float value = QString::fromStdString(params.get("LagdToggledelay")).toFloat(); + delay_control->setLabel(QString::number(value, 'f', 2) + "s"); + delay_control->showDescription(); + } } /** diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h index 8b8cf6975..61f15e6d3 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h @@ -64,6 +64,8 @@ private: bool is_onroad = false; ButtonControlSP *currentModelLblBtn; + ParamControlSP *lagd_toggle_control; + OptionControlSP *delay_control; QProgressBar *supercomboProgressBar; QFrame *supercomboFrame; QProgressBar *navigationProgressBar; diff --git a/selfdrive/ui/sunnypilot/qt/widgets/controls.h b/selfdrive/ui/sunnypilot/qt/widgets/controls.h index c8849faf5..29b6e6e6e 100644 --- a/selfdrive/ui/sunnypilot/qt/widgets/controls.h +++ b/selfdrive/ui/sunnypilot/qt/widgets/controls.h @@ -480,6 +480,16 @@ private: return result.toInt(); } + int getParamValueScaled() { + const auto param_value = QString::fromStdString(params.get(key)); + return static_cast(param_value.toFloat() * 100); + } + + void setParamValueScaled(const int new_value) { + const float scaled_value = new_value / 100.0f; + params.put(key, QString::number(scaled_value, 'f', 2).toStdString()); + } + // Although the method is not static, and thus has access to the value property, I prefer to be explicit about the value. void setParamValue(const int new_value) { const auto value_str = valueMap != nullptr ? valueMap->value(QString::number(new_value)) : QString::number(new_value); @@ -488,7 +498,8 @@ private: public: OptionControlSP(const QString ¶m, const QString &title, const QString &desc, const QString &icon, - const MinMaxValue &range, const int per_value_change = 1, const bool inline_layout = false, const QMap *valMap = nullptr) : AbstractControlSP_SELECTOR(title, desc, icon, nullptr), _title(title), valueMap(valMap), is_inline_layout(inline_layout) { + const MinMaxValue &range, const int per_value_change = 1, const bool inline_layout = false, + const QMap *valMap = nullptr, bool scale_float = false) : AbstractControlSP_SELECTOR(title, desc, icon, nullptr), _title(title), valueMap(valMap), is_inline_layout(inline_layout), use_float_scaling(scale_float) { const QString style = R"( QPushButton { border-radius: 20px; @@ -528,7 +539,7 @@ public: const std::vector button_texts{"－", "＋"}; key = param.toStdString(); - value = getParamValue(); + value = use_float_scaling ? getParamValueScaled() : getParamValue(); button_group = new QButtonGroup(this); button_group->setExclusive(true); @@ -546,10 +557,15 @@ public: QObject::connect(button, &QPushButton::clicked, [=]() { int change_value = (i == 0) ? -per_value_change : per_value_change; - value = getParamValue(); // in case it changed externally, we need to get the latest value. + value = use_float_scaling ? getParamValueScaled() : getParamValue(); value += change_value; value = std::clamp(value, range.min_value, range.max_value); - setParamValue(value); + + if (use_float_scaling) { + setParamValueScaled(value); + } else { + setParamValue(value); + } button_group->button(0)->setEnabled(!(value <= range.min_value)); button_group->button(1)->setEnabled(!(value >= range.max_value)); @@ -642,6 +658,7 @@ private: const QString label_disabled_style = "font-size: 50px; font-weight: 450; color: #5C5C5C;"; bool button_enabled = true; + bool use_float_scaling = false; }; class PushButtonSP : public QPushButton { diff --git a/sunnypilot/livedelay/__init__.py b/sunnypilot/livedelay/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/sunnypilot/livedelay/lagd_toggle.py b/sunnypilot/livedelay/lagd_toggle.py new file mode 100644 index 000000000..cc2f1e250 --- /dev/null +++ b/sunnypilot/livedelay/lagd_toggle.py @@ -0,0 +1,51 @@ +""" +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 +from openpilot.common.swaglog import cloudlog + + +class LagdToggle: + def __init__(self): + self.params = Params() + self.lag = 0.0 + self._last_desc = None + + @property + def software_delay(self): + return float(self.params.get("LagdToggledelay", encoding='utf8')) + + def _maybe_update_desc(self, desc): + if desc != self._last_desc: + self.params.put_nonblocking("LagdToggleDesc", desc) + self._last_desc = desc + + 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 + + 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 + + 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 d4bd96ff6..86968ea26 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -25,7 +25,7 @@ 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.models.modeld_lagd import ModeldLagd +from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext @@ -202,7 +202,7 @@ def main(demo=False): cloudlog.info("modeld got CarParams: %s", CP.brand) - modeld_lagd = ModeldLagd() + modeld_lagd = LagdToggle() # Enable lagd support for sunnypilot modeld long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index 5b9c5ef9e..58e79eceb 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -23,7 +23,7 @@ from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFr from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants from openpilot.sunnypilot.models.helpers import get_active_bundle -from openpilot.sunnypilot.models.modeld_lagd import ModeldLagd +from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle from openpilot.sunnypilot.models.runners.helpers import get_model_runner PROCESS_NAME = "selfdrive.modeld.modeld" @@ -239,7 +239,7 @@ def main(demo=False): cloudlog.info("modeld got CarParams: %s", CP.brand) - modeld_lagd = ModeldLagd() + modeld_lagd = LagdToggle() # TODO Move smooth seconds to action function long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS diff --git a/sunnypilot/models/modeld_lagd.py b/sunnypilot/models/modeld_lagd.py deleted file mode 100644 index 9e538123a..000000000 --- a/sunnypilot/models/modeld_lagd.py +++ /dev/null @@ -1,26 +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 -from openpilot.common.swaglog import cloudlog - - -class ModeldLagd: - def __init__(self): - self.params = 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 - cloudlog.debug(f"LAGD USING LIVE DELAY: {lateral_delay:.3f} + {lat_smooth:.3f} = {result:.3f}") - return result - steer_actuator_delay = CP.steerActuatorDelay - lat_smooth = model.LAT_SMOOTH_SECONDS - result = (steer_actuator_delay + 0.2) + lat_smooth - cloudlog.debug(f"LAGD USING STEER ACTUATOR: {steer_actuator_delay:.3f} + 0.2 + {lat_smooth:.3f} = {result:.3f}") - return result diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py index 8ccd81d8f..b3c2d78ac 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.use_steering_angle = lac_torque.use_steering_angle diff --git a/system/manager/manager.py b/system/manager/manager.py index 9de332df4..692c7717f 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -57,6 +57,7 @@ def manager_init() -> None: ("DynamicExperimentalControl", "0"), ("HyundaiLongitudinalTuning", "0"), ("LagdToggle", "1"), + ("LagdToggledelay", "0.2"), ("Mads", "1"), ("MadsMainCruiseAllowed", "1"), ("MadsSteeringMode", "0"),