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