From d18158781272a31077788c55c5e7cebc27360d1b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Mon, 31 Mar 2025 21:04:02 -0700 Subject: [PATCH 1/4] Use in modeld --- selfdrive/modeld/modeld.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 1e557942fe..8c3c8ba4d6 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -195,7 +195,7 @@ def main(demo=False): # messaging pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) - sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"]) + sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) publish_state = PublishState() params = Params() @@ -220,9 +220,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) - # TODO this needs more thought, use .2s extra for now to estimate other delays - steer_delay = CP.steerActuatorDelay + .2 - DH = DesireHelper() while True: @@ -263,6 +260,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 lateral_control_params = np.array([v_ego, steer_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) From 4ffd617b20cba8d82a249ecb552af5aa9dac7b89 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Mon, 31 Mar 2025 21:06:13 -0700 Subject: [PATCH 2/4] Use in torqued --- selfdrive/locationd/torqued.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index fcaa19df73..144a0a8b44 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -175,7 +175,8 @@ class TorqueEstimator(ParameterEstimator): self.raw_points["steer_override"].append(msg.steeringPressed) elif which == "liveCalibration": self.calibrator.feed_live_calib(msg) - + elif which == "liveDelay": + self.lag = msg.lateralDelay # calculate lateral accel from past steering torque elif which == "livePose": if len(self.raw_points['steer_torque']) == self.hist_len: @@ -241,7 +242,7 @@ def main(demo=False): config_realtime_process([0, 1, 2, 3], 5) pm = messaging.PubMaster(['liveTorqueParameters']) - sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose'], poll='livePose') + sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose', 'liveDelay'], poll='livePose') params = Params() estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) From 88991318c007e51ad8e851a7b323b5731e2bd7e3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Mon, 31 Mar 2025 21:07:06 -0700 Subject: [PATCH 3/4] Do not use default value in torqued --- selfdrive/locationd/torqued.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 144a0a8b44..5ed86e457f 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -52,7 +52,7 @@ class TorqueBuckets(PointBuckets): class TorqueEstimator(ParameterEstimator): def __init__(self, CP, decimated=False, track_all_points=False): self.hist_len = int(HISTORY / DT_MDL) - self.lag = CP.steerActuatorDelay + .2 # from controlsd + self.lag = 0.0 self.track_all_points = track_all_points # for offline analysis, without max lateral accel or max steer torque filters if decimated: self.min_bucket_points = MIN_BUCKET_POINTS / 10 From 5e5ccfe05722cb4dff72323a0137a04b76e0ecbe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Wed, 9 Apr 2025 15:39:13 -0700 Subject: [PATCH 4/4] Add liveDelay in process replay --- selfdrive/test/process_replay/process_replay.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 9715e48615..08870953d3 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -568,7 +568,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="torqued", - pubs=["livePose", "liveCalibration", "carState", "carControl", "carOutput"], + pubs=["livePose", "liveCalibration", "liveDelay", "carState", "carControl", "carOutput"], subs=["liveTorqueParameters"], ignore=["logMonoTime"], init_callback=get_car_params_callback, @@ -577,7 +577,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="modeld", - pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState", "carState", "carControl"], + pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "liveDelay", "driverMonitoringState", "carState", "carControl"], subs=["modelV2", "drivingModelData", "cameraOdometry"], ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime", "drivingModelData.frameDropPerc", "drivingModelData.modelExecutionTime"], should_recv_callback=ModeldCameraSyncRcvCallback(),