From e581322de5faaf9c3645f1758d122eabd60552f3 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 5 Mar 2024 14:02:02 -0500 Subject: [PATCH] torqued: use correct time from carOutput (#31712) fix timings old-commit-hash: 9d4d5f6077a4bb0c40eb91ec41154f7903761253 --- selfdrive/locationd/torqued.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index b49784d13f..06f9044738 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -161,6 +161,7 @@ class TorqueEstimator(ParameterEstimator): self.raw_points["carControl_t"].append(t + self.lag) self.raw_points["active"].append(msg.latActive) elif which == "carOutput": + self.raw_points["carOutput_t"].append(t + self.lag) self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer) elif which == "carState": self.raw_points["carState_t"].append(t + self.lag) @@ -173,7 +174,7 @@ class TorqueEstimator(ParameterEstimator): active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carControl_t'], self.raw_points['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) vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego']) - steer = np.interp(t, self.raw_points['carControl_t'], self.raw_points['steer_torque']) + steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque']) lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY) if all(active) and (not any(steer_override)) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD): self.filtered_points.add_point(float(steer), float(lateral_acc))