48 lines
1.3 KiB
Python
48 lines
1.3 KiB
Python
import serial
|
|
import struct
|
|
import math
|
|
|
|
# --- impostazioni ---
|
|
G = 9.80665
|
|
ACC_LSB = 2048.0
|
|
GYRO_LSB = 16.4
|
|
|
|
def acc_raw_to_ms2(raw):
|
|
return (raw / ACC_LSB) * G
|
|
|
|
def gyro_raw_to_rad(raw):
|
|
return (raw / GYRO_LSB) * (math.pi / 180.0)
|
|
|
|
# apri seriale
|
|
ser = serial.Serial('COM8', 2000000)
|
|
|
|
frame_format = '<HIhhhhhhHH' # magic, ts, imu1(ax..gz), imu2(ax..gz), crc
|
|
frame_size = struct.calcsize(frame_format)
|
|
|
|
while True:
|
|
buf = ser.read(frame_size)
|
|
if len(buf) != frame_size:
|
|
continue
|
|
|
|
unpacked = struct.unpack(frame_format, buf)
|
|
magic, ts = unpacked[0], unpacked[1]
|
|
imu1_raw = unpacked[2:8]
|
|
imu2_raw = unpacked[8:14]
|
|
|
|
# conversione accelerometro e giroscopio
|
|
imu1 = [round(acc_raw_to_ms2(imu1_raw[0]), 2),
|
|
round(acc_raw_to_ms2(imu1_raw[1]), 2),
|
|
round(acc_raw_to_ms2(imu1_raw[2]), 2),
|
|
round(gyro_raw_to_rad(imu1_raw[3]), 2),
|
|
round(gyro_raw_to_rad(imu1_raw[4]), 2),
|
|
round(gyro_raw_to_rad(imu1_raw[5]), 2)]
|
|
|
|
imu2 = [round(acc_raw_to_ms2(imu2_raw[0]), 2),
|
|
round(acc_raw_to_ms2(imu2_raw[1]), 2),
|
|
round(acc_raw_to_ms2(imu2_raw[2]), 2),
|
|
round(gyro_raw_to_rad(imu2_raw[3]), 2),
|
|
round(gyro_raw_to_rad(imu2_raw[4]), 2),
|
|
round(gyro_raw_to_rad(imu2_raw[5]), 2)]
|
|
|
|
print(f"t={ts} imu1={imu1} imu2={imu2}")
|