mawei
/
tsl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

torqued: check steer override to current time (#32963) 

* lat active/steer override: check up to now

* lint

* Update ref_commit
old-commit-hash: b247c3caaa
This commit is contained in:
Shane Smiskol 2024-07-10 17:25:39 -07:00 committed by GitHub
parent d7f77d0dc7
commit bb67139ca7
2 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
selfdrive/locationd/torqued.py
View File

@ -177,8 +177,11 @@ class TorqueEstimator(ParameterEstimator):
if len(self.raw_points['steer_torque']) == self.hist_len: if len(self.raw_points['steer_torque']) == self.hist_len:
yaw_rate = msg.angularVelocityCalibrated.value[2] yaw_rate = msg.angularVelocityCalibrated.value[2]
roll = msg.orientationNED.value[0] 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) # check lat active up to now (without lag compensation)
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) 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']) 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() 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() lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY).item()

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
f6ff3601bd0496e78d8bc3b019d58bb7739f096b 0adff03d45c99dcfb330c48b2aa9d2093ce674a2