From 2ed6f9bfa99f891f4270fc2a378bcef628f19232 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Tue, 4 Feb 2025 20:03:30 -0800 Subject: [PATCH] Fix issues --- selfdrive/locationd/torqued.py | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 5d2be5e322..7e06e57cc6 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -48,9 +48,9 @@ MIN_LAG_VEL = 15.0 class LagEstimator(ParameterEstimator): def __init__(self, CP): - self.hist_len = int(MIN_HIST_LEN_SEC / DT_CTRL) - self.min_hist_len = int(MIN_HIST_LEN_SEC / DT_CTRL) - self.initial_lag = CP.steerActuatorDelay + self.hist_len = int(MIN_HIST_LEN_SEC / DT_MDL) + self.min_hist_len = int(MIN_HIST_LEN_SEC / DT_MDL) + self.initial_lag = -CP.steerActuatorDelay self.current_lag = self.initial_lag self.lat_active = False @@ -94,9 +94,10 @@ class LagEstimator(ParameterEstimator): if len(self.points['curvature']) >= self.min_hist_len and self.v_ego > MIN_LAG_VEL: _, curvature = zip(*self.points['curvature']) _, desired_curvature = zip(*self.points['desired_curvature']) - delay_curvature, _ = self.actuator_delay(curvature, desired_curvature, DT_CTRL) + delay_curvature, _ = self.actuator_delay(desired_curvature, curvature, DT_MDL) - steer_actuation_delay = (self.current_lag + delay_curvature) / 2. + print(delay_curvature) + steer_actuation_delay = float((self.current_lag + delay_curvature) / 2.) self.current_lag = steer_actuation_delay @@ -309,11 +310,14 @@ class TorqueEstimator(ParameterEstimator): 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') + pm = messaging.PubMaster(['liveTorqueParameters', 'liveActuatorDelay']) + sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'controlsState', 'liveCalibration', 'livePose'], poll='livePose') params = Params() - estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) + CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) + estimator = TorqueEstimator(CP) + + lag_estimator = LagEstimator(CP) while True: sm.update() @@ -322,10 +326,12 @@ def main(demo=False): if sm.updated[which]: t = sm.logMonoTime[which] * 1e-9 estimator.handle_log(t, which, sm[which]) + lag_estimator.handle_log(t, which, sm[which]) # 4Hz driven by livePose if sm.frame % 5 == 0: - pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks())) + pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose']))) + pm.send('liveActuatorDelay', lag_estimator.get_msg(valid=sm.all_checks(['carControl', 'carState', 'controlsState']), with_points=False)) # Cache points every 60 seconds while onroad if sm.frame % 240 == 0: