Reapply "LagdToggle: refactor and only instantiate once" (commaai#1137) (commaai#1138)

* Reapply "`LagdToggle`: refactor and only instantiate once" (commaai#1137)

This reverts commit b4f19d4.

* infinite woo gone

* use them hz

Author: sunnyhaibin

common/params_keys.h

@@ -192,8 +192,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},

selfdrive/controls/controlsd.py

@@ -21,6 +21,8 @@
 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 @@
 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 @@ def state_control(self):
                                            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']

selfdrive/locationd/lagd.py

@@ -12,6 +12,7 @@
 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)

selfdrive/locationd/torqued.py

@@ -10,8 +10,7 @@
 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 @@ def add_point(self, x, y):
         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 @@ def __init__(self, CP, decimated=False, track_all_points=False):
 
     # 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 @@ def handle_log(self, t, which, msg):
     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:

selfdrive/modeld/modeld.py

@@ -31,7 +31,8 @@
 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 @@ def __init__(self, vipc=None):
     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)

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 += "<br><br><b><span style=\"color:#e0e0e0\">" + tr("Current:") + "</span></b> <span style=\"color:#e0e0e0\">" + current + "</span>";
-  }
   lagd_toggle_control->setDescription(desc);
 
   delay_control->setVisible(!params.getBool("LagdToggle"));

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

sunnypilot/livedelay/lagd_toggle.py

@@ -4,48 +4,35 @@
 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")
-
-  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
+
+    self.lagd_toggle = self.params.get_bool("LagdToggle")
+    self.software_delay = self.params.get("LagdToggleDelay", return_default=True)
+
+  def read_params(self) -> None:
+    self.lagd_toggle = self.params.get_bool("LagdToggle")
+    self.software_delay = self.params.get("LagdToggleDelay", return_default=True)
+
+  def update(self, lag_msg: log.LiveDelayData) -> None:
+    self.read_params()
+
+    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
+
+    lateral_delay = lag_msg.liveDelay.lateralDelay
+    self.lag = lateral_delay
+    self.params.put_nonblocking("LagdValueCache", self.lag)

sunnypilot/modeld/modeld.py

@@ -20,13 +20,14 @@
 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 @@ def __init__(self, vipc=None):
     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]

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)