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

@@ -178,6 +178,8 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
 
     // model panel params
     {"LagdToggle", PERSISTENT | BACKUP},
+    {"LagdToggleDesc", PERSISTENT},
+    {"LagdToggledelay", PERSISTENT | BACKUP},
 
     // mapd
     {"MapAdvisorySpeedLimit", CLEAR_ON_ONROAD_TRANSITION},

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']

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:

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)

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 += "<br><br><b><span style=\"color:#e0e0e0\">" + tr("Current:") + "</span></b> <span style=\"color:#e0e0e0\">" + current + "</span>";
+  }
+  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();
+  }
 }
 
 /**

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;

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<int>(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 &param, 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<QString, QString> *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<QString, QString> *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<QString> 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 {

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

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

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

sunnypilot/models/modeld_lagd.py

@@ -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

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

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"),