diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index fcaa19df73..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 @@ -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)) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 9805506432..25bb190fcd 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -220,7 +220,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() @@ -247,7 +247,6 @@ def main(demo=False): # TODO this needs more thought, use .2s extra for now to estimate other delays # TODO Move smooth seconds to action function - lat_delay = CP.steerActuatorDelay + .2 + LAT_SMOOTH_SECONDS long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS prev_action = log.ModelDataV2.Action() @@ -291,6 +290,7 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) + lat_delay = sm["liveDelay"].lateralDelay + 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) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 6139846322..3183db0c3a 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(),