mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-09-15 03:53:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

paramsd/torqued: use the correct livePose timestamp (#37704)

Browse Source 
* Use the correct filter time in torqued/paramsd

* Fix

* Check if lp valid

* Update tests fake data with new required fields
This commit is contained in:
Kacper Rączy
2026-03-20 19:10:59 -07:00
committed by GitHub
parent 08d8bb9975
commit 7fae59167e
3 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/tests/test_torqued_lat_accel_offset.py
+4 -2
View File
@@ -50,8 +50,10 @@ def simulate_straight_road_msgs(est):
lat_accels = TORQUE_TUNE.latAccelFactor * steer_torques
for t, steer_torque, lat_accel in zip(ts, steer_torques, lat_accels, strict=True):
carOutput.actuatorsOutput.torque = float(-steer_torque)
livePose.orientationNED.x = float(np.deg2rad(ROLL_BIAS_DEG))
livePose.angularVelocityDevice.z = float(lat_accel / V_EGO)
livePose.orientationNED = {'x': float(np.deg2rad(ROLL_BIAS_DEG)), 'valid': True}
livePose.angularVelocityDevice = {'z': float(lat_accel / V_EGO), 'valid': True}
livePose.inputsOK, livePose.sensorsOK, livePose.posenetOK = True, True, True
livePose.timestamp = int(t * 1e9)
for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('livePose', livePose)):
est.handle_log(t, which, msg)
selfdrive/locationd/paramsd.py
+1
View File
@@ -65,6 +65,7 @@ class VehicleParamsLearner:
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
if which == 'livePose':
t = msg.timestamp * 1e-9
device_pose = Pose.from_live_pose(msg)
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
selfdrive/locationd/torqued.py
+3 -1
View File
@@ -180,7 +180,9 @@ class TorqueEstimator(ParameterEstimator):
self.lag = msg.lateralDelay
# calculate lateral accel from past steering torque
elif which == "livePose":
if len(self.raw_points['steer_torque']) == self.hist_len:
is_valid = msg.angularVelocityDevice.valid and msg.orientationNED.valid and msg.inputsOK and msg.sensorsOK and msg.posenetOK
if len(self.raw_points['steer_torque']) == self.hist_len and is_valid:
t = msg.timestamp * 1e-9
device_pose = Pose.from_live_pose(msg)
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
angular_velocity_calibrated = calibrated_pose.angular_velocity