From bb67139ca772f3aa94a7a57b087a19f8b339e28f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 10 Jul 2024 17:25:39 -0700 Subject: [PATCH] torqued: check steer override to current time (#32963) * lat active/steer override: check up to now * lint * Update ref_commit old-commit-hash: b247c3caaa5b5d503a9c1aa3336a7a206765167c --- selfdrive/locationd/torqued.py | 7 +++++-- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index ba649b318..e0784eb8e 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -177,8 +177,11 @@ class TorqueEstimator(ParameterEstimator): if len(self.raw_points['steer_torque']) == self.hist_len: yaw_rate = msg.angularVelocityCalibrated.value[2] roll = msg.orientationNED.value[0] - lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool) - steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carState_t'], self.raw_points['steer_override']).astype(bool) + # check lat active up to now (without lag compensation) + lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL), + self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool) + steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL), + self.raw_points['carState_t'], self.raw_points['steer_override']).astype(bool) vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego']) steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque']).item() lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY).item() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1635012f3..13b427b89 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f6ff3601bd0496e78d8bc3b019d58bb7739f096b \ No newline at end of file +0adff03d45c99dcfb330c48b2aa9d2093ce674a2