diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 5ed86e457f..fcaa19df73 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 = 0.0 + self.lag = CP.steerActuatorDelay + .2 # from controlsd 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,8 +175,7 @@ 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: @@ -242,7 +241,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', 'liveDelay'], poll='livePose') + sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose'], 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 1e3d1782e6..96520a4b15 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -229,7 +229,7 @@ def main(demo=False): # messaging pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) - sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) + sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"]) publish_state = PublishState() params = Params() @@ -256,6 +256,7 @@ 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() @@ -299,7 +300,6 @@ 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 4f8aa9d99d..2b0a6f8965 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -564,7 +564,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="torqued", - pubs=["livePose", "liveCalibration", "liveDelay", "carState", "carControl", "carOutput"], + pubs=["livePose", "liveCalibration", "carState", "carControl", "carOutput"], subs=["liveTorqueParameters"], ignore=["logMonoTime"], init_callback=get_car_params_callback, @@ -573,7 +573,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="modeld", - pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "liveDelay", "driverMonitoringState", "carState", "carControl"], + pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState", "carState", "carControl"], subs=["modelV2", "drivingModelData", "cameraOdometry"], ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime", "drivingModelData.frameDropPerc", "drivingModelData.modelExecutionTime"], should_recv_callback=ModeldCameraSyncRcvCallback(),