diff --git a/selfdrive/controls/tests/test_torqued_lat_accel_offset.py b/selfdrive/controls/tests/test_torqued_lat_accel_offset.py index 84389856b..2f95d7c14 100644 --- a/selfdrive/controls/tests/test_torqued_lat_accel_offset.py +++ b/selfdrive/controls/tests/test_torqued_lat_accel_offset.py @@ -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) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index fd03d3d09..0489ae417 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -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) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index f4dc5a147..9a2b6c17b 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -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