diff --git a/common/params_keys.h b/common/params_keys.h index 4776120c0..2e967a339 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -192,8 +192,8 @@ inline static std::unordered_map 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}}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6f653a788..61ad4754a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,8 +21,6 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque 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 @@ LaneChangeDirection = log.LaneChangeDirection 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 @@ class Controls(ControlsExt, ModelStateBase): 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'] diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 7f1d5fd03..d7834f7f1 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -12,7 +12,6 @@ from openpilot.common.params import Params 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) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index ee12f2c36..006670831 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -10,7 +10,8 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL 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 @@ 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 @@ -98,7 +99,6 @@ class TorqueEstimator(ParameterEstimator): # 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 @@ class TorqueEstimator(ParameterEstimator): 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: diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 13f4b6e12..5c2ce9059 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -31,8 +31,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan 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 @@ class FrameMeta: 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, diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc index 0699deef0..45a22d7e0 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc +++ b/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 += "

" + tr("Current:") + " " + current + ""; + } lagd_toggle_control->setDescription(desc); delay_control->setVisible(!params.getBool("LagdToggle")); diff --git a/sunnypilot/livedelay/helpers.py b/sunnypilot/livedelay/helpers.py deleted file mode 100644 index fe0760c1c..000000000 --- a/sunnypilot/livedelay/helpers.py +++ /dev/null @@ -1,14 +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 - - -def get_lat_delay(params: Params, cur_val: float, updated: bool) -> float: - if updated and params.get_bool("LagdToggle"): - return float(params.get("LagdValueCache", return_default=True)) - - return cur_val diff --git a/sunnypilot/livedelay/lagd_toggle.py b/sunnypilot/livedelay/lagd_toggle.py index 8495dcee0..ee135fb87 100644 --- a/sunnypilot/livedelay/lagd_toggle.py +++ b/sunnypilot/livedelay/lagd_toggle.py @@ -4,35 +4,48 @@ 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 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._last_desc = None - self.lagd_toggle = self.params.get_bool("LagdToggle") - self.software_delay = self.params.get("LagdToggleDelay", return_default=True) + @property + def software_delay(self): + return self.params.get("LagdToggleDelay") - def read_params(self) -> None: - self.lagd_toggle = self.params.get_bool("LagdToggle") - self.software_delay = self.params.get("LagdToggleDelay", return_default=True) + def _maybe_update_desc(self, desc): + if desc != self._last_desc: + self.params.put_nonblocking("LagdToggleDesc", desc) + self._last_desc = desc - def update(self, lag_msg: log.LiveDelayData) -> None: - self.read_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 + 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 - 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 + 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 - lateral_delay = lag_msg.liveDelay.lateralDelay - self.lag = lateral_delay - self.params.put_nonblocking("LagdValueCache", self.lag) + 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 8e20e6b62..10725efb7 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -20,14 +20,13 @@ from openpilot.system import sentry 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 @@ class FrameMeta: 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] diff --git a/sunnypilot/modeld/modeld_base.py b/sunnypilot/modeld/modeld_base.py deleted file mode 100644 index ba57659b0..000000000 --- a/sunnypilot/modeld/modeld_base.py +++ /dev/null @@ -1,12 +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 - - -class ModelStateBase: - def __init__(self): - self.lat_delay = Params().get("LagdValueCache", return_default=True) diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index cec54c003..93a8a3f05 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -22,9 +22,8 @@ from openpilot.sunnypilot.modeld_v2.constants import Plan from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLContext from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants -from openpilot.sunnypilot.livedelay.helpers import get_lat_delay -from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase from openpilot.sunnypilot.models.helpers import get_active_bundle +from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle from openpilot.sunnypilot.models.runners.helpers import get_model_runner PROCESS_NAME = "selfdrive.modeld.modeld" @@ -40,14 +39,13 @@ class FrameMeta: 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] prev_desire: np.ndarray # for tracking the rising edge of the pulse temporal_idxs: slice | np.ndarray def __init__(self, context: CLContext): - ModelStateBase.__init__(self) try: self.model_runner = get_model_runner() self.constants = self.model_runner.constants @@ -244,6 +242,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 = LagdToggle() + # TODO Move smooth seconds to action function long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS prev_action = log.ModelDataV2.Action() @@ -288,7 +289,7 @@ 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"]) + 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))] @@ -325,7 +326,7 @@ def main(demo=False): } if "lateral_control_params" in model.numpy_inputs.keys(): - inputs['lateral_control_params'] = np.array([v_ego, model.lat_delay], dtype=np.float32) + inputs['lateral_control_params'] = np.array([v_ego, steer_delay], dtype=np.float32) mt1 = time.perf_counter() model_output = model.run(bufs, transforms, inputs, prepare_only) @@ -337,7 +338,7 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - action = model.get_action_from_model(model_output, prev_action, model.lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) + action = model.get_action_from_model(model_output, prev_action, steer_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, diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py index 6a39b3172..43d652e8b 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.torque_params = lac_torque.torque_params