parent
7ee5fb7e66
commit
b3a868103a
|
@ -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):
|
||||
|
|
|
@ -1 +1 @@
|
|||
defb3dc7205a3d501829f4a780241dfb97132025
|
||||
0e2d73270816b93dfbd4c2f673591a7ba492edac
|
Loading…
Reference in New Issue