From 0e6e652521e3b6bb3ccb33ecd7284252d478e9a2 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Mon, 8 Feb 2021 22:15:05 -0800 Subject: [PATCH] fix timestamps (#20029) * fix timestamps * nanoseconds * update refs old-commit-hash: b3a868103a479aad2d4f787e16ba6f1828897104 --- selfdrive/locationd/locationd.py | 5 +++-- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 6f4443d82..896786a28 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -240,6 +240,7 @@ class Localizer(): def handle_sensors(self, current_time, log): # TODO does not yet account for double sensor readings in the log for sensor_reading in log: + sensor_time = 1e-9 * sensor_reading.timestamp # TODO: handle messages from two IMUs at the same time if sensor_reading.source == SensorSource.lsm6ds3: continue @@ -249,7 +250,7 @@ class Localizer(): self.gyro_counter += 1 if self.gyro_counter % SENSOR_DECIMATION == 0: v = sensor_reading.gyroUncalibrated.v - self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]]) + self.update_kalman(sensor_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]]) # Accelerometer if sensor_reading.sensor == 1 and sensor_reading.type == 1: @@ -260,7 +261,7 @@ class Localizer(): self.acc_counter += 1 if self.acc_counter % SENSOR_DECIMATION == 0: v = sensor_reading.acceleration.v - self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]]) + self.update_kalman(sensor_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]]) def handle_live_calib(self, current_time, log): if len(log.rpyCalib): diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 2c873e62d..21d8f8037 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -defb3dc7205a3d501829f4a780241dfb97132025 \ No newline at end of file +0e2d73270816b93dfbd4c2f673591a7ba492edac \ No newline at end of file