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

Revert "`LagdToggle`: refactor and only instantiate once (commaai#1130)"

This reverts commit 6ae668e.

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

selfdrive/locationd/lagd.py

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

selfdrive/locationd/torqued.py

@@ -10,7 +10,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.helpers import get_lat_delay
+
+from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle
 
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
@@ -50,7 +51,7 @@ def add_point(self, x, y):
         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 @@ 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 = 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:

selfdrive/modeld/modeld.py

@@ -31,8 +31,7 @@
 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 @@ 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(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,

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 += "<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

@@ -4,35 +4,48 @@
 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.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)
+    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

sunnypilot/livedelay/lagd_toggle.py

@@ -20,14 +20,13 @@
 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 @@ 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(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]