ui/models panel: liveDelay Toggle (#971)

* Model panel

* Get this outta here and use sunnylink instead

* Remove space

* 'not' because we want gas gating to use e2e/mpc blend

* Add toggles to models_panel.cc

* "keep enabled for stock behavior"

* Add this here

* Cloudlog result

* change cloudlog to debug, and add latsmooth to steer actuator delay.

Need to edit json. Will do locally so when this is merged its a simple ready to go push.

* Cleanup

* Update longitudinal_planner.py

* Remove gasgating for now.. may need to be placed in model

---------

Co-authored-by: DevTekVE <devtekve@gmail.com>

common/params_keys.h

@@ -71,7 +71,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"LastPowerDropDetected", CLEAR_ON_MANAGER_START},
     {"LastUpdateException", CLEAR_ON_MANAGER_START},
     {"LastUpdateTime", PERSISTENT},
-    {"LiveDelay", PERSISTENT},
+    {"LiveDelay", PERSISTENT | BACKUP},
     {"LiveParameters", PERSISTENT},
     {"LiveParametersV2", PERSISTENT},
     {"LiveTorqueParameters", PERSISTENT | DONT_LOG},
@@ -170,4 +170,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"HyundaiLongitudinalTuning", PERSISTENT},
 
     {"DynamicExperimentalControl", PERSISTENT},
+
+    // model panel params
+    {"LagdToggle", PERSISTENT | BACKUP},
 };

selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc

@@ -31,6 +31,13 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
     });
   connect(uiStateSP(), &UIStateSP::uiUpdate, this, &ModelsPanel::updateLabels);
   list->addItem(currentModelLblBtn);
+
+  // 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"));
 }
 
 

sunnypilot/modeld/modeld.py

@@ -25,6 +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.modeld.models.commonmodel_pyx import ModelFrame, CLContext
 
 
@@ -201,8 +202,9 @@ def main(demo=False):
 
   cloudlog.info("modeld got CarParams: %s", CP.brand)
 
+  modeld_lagd = ModeldLagd()
+
   # Enable lagd support for sunnypilot modeld
-  steer_delay = sm["liveDelay"].lateralDelay + model.LAT_SMOOTH_SECONDS
   long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS
   prev_action = log.ModelDataV2.Action()
 
@@ -246,6 +248,8 @@ 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.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))]

sunnypilot/modeld_v2/modeld.py

@@ -23,6 +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.models.runners.helpers import get_model_runner
 
 PROCESS_NAME = "selfdrive.modeld.modeld"
@@ -238,6 +239,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 = ModeldLagd()
+
   # TODO Move smooth seconds to action function
   long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS
   prev_action = log.ModelDataV2.Action()
@@ -282,7 +286,7 @@ def main(demo=False):
     is_rhd = sm["driverMonitoringState"].isRHD
     frame_id = sm["roadCameraState"].frameId
     v_ego = max(sm["carState"].vEgo, 0.)
-    steer_delay = sm["liveDelay"].lateralDelay + 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))]

sunnypilot/models/modeld_lagd.py

@@ -0,0 +1,26 @@
+"""
+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

system/manager/manager.py

@@ -48,6 +48,7 @@ def manager_init() -> None:
     ("BlinkerPauseLateralControl", "0"),
     ("DynamicExperimentalControl", "0"),
     ("HyundaiLongitudinalTuning", "0"),
+    ("LagdToggle", "1"),
     ("Mads", "1"),
     ("MadsMainCruiseAllowed", "1"),
     ("MadsSteeringMode", "0"),