45 lines
1.4 KiB
Python
45 lines
1.4 KiB
Python
import serial
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
|
|
ser = serial.Serial(port='/dev/ttyACM1', timeout = 1)
|
|
ser.flush()
|
|
ser.reset_input_buffer()
|
|
|
|
gyros = []
|
|
accs = []
|
|
|
|
fig, axs = plt.subplots(2)
|
|
i = 0
|
|
|
|
while(1):
|
|
x = [int(e, 16) for e in ser.readline()[1:-1].split(b" ")]
|
|
imu_reading_type = x[0] >> 3
|
|
imu_reading_tag_cnt = (x[0] >> 1) & 3
|
|
#print(imu_reading_type, imu_reading_tag_cnt)
|
|
reading_xyz = [int.from_bytes(x[1:3], byteorder = 'little', signed = True),
|
|
int.from_bytes(x[3:5], byteorder = 'little', signed = True),
|
|
int.from_bytes(x[5:7], byteorder = 'little', signed = True)]
|
|
i += 1
|
|
if imu_reading_type == 1:
|
|
gyros.append([250 * e / (1 << 16) for e in reading_xyz])
|
|
elif imu_reading_type == 2:
|
|
accs.append([4 * e / (1<<16) for e in reading_xyz])
|
|
|
|
if len(gyros) > 50 and (i % 10) == 0:
|
|
gyros = gyros[-100:]
|
|
accs = accs[-100:]
|
|
a = np.array(accs)
|
|
g = np.array(gyros)
|
|
a -= np.mean(a, axis = 0).reshape(1, 3)
|
|
g -= np.mean(g, axis = 0).reshape(1, 3)
|
|
axs[0].cla()
|
|
axs[0].plot(g[:,0])
|
|
axs[0].plot(g[:,1])
|
|
axs[0].plot(g[:,2])
|
|
axs[1].cla()
|
|
axs[1].plot(a[:,0])
|
|
axs[1].plot(a[:,1])
|
|
axs[1].plot(a[:,2])
|
|
plt.pause(0.0001)
|