82 lines
2.7 KiB
Python
82 lines
2.7 KiB
Python
import serial
|
||
import time
|
||
|
||
def checksum(data):
|
||
return sum(data[:-1]) & 0xFF
|
||
|
||
def parse_packet(data):
|
||
if len(data) != 11:
|
||
return None
|
||
if data[0] != 0x55:
|
||
return None
|
||
if checksum(data) != data[-1]:
|
||
print("校验失败")
|
||
return None
|
||
|
||
packet_type = data[1]
|
||
# 将后8字节分成4个16位有符号整数（小端序）
|
||
vals = [int.from_bytes(data[i:i+2], 'little', signed=True) for i in range(2, 10, 2)]
|
||
|
||
if packet_type == 0x51: # 加速度，单位0.001g
|
||
ax, ay, az, temp = vals
|
||
ax /= 1000
|
||
ay /= 1000
|
||
az /= 1000
|
||
temp = temp / 100 # 温度单位摄氏度
|
||
# return f"加速度 (g): x={ax:.3f}, y={ay:.3f}, z={az:.3f}, 温度={temp:.2f}℃"
|
||
elif packet_type == 0x52: # 角速度，单位0.01°/s
|
||
wx, wy, wz, temp = vals
|
||
wx /= 100
|
||
wy /= 100
|
||
wz /= 100
|
||
temp = temp / 100
|
||
# return f"角速度 (°/s): x={wx:.2f}, y={wy:.2f}, z={wz:.2f}, 温度={temp:.2f}℃"
|
||
elif packet_type == 0x53: # 姿态角，单位0.01°
|
||
roll, pitch, yaw, temp = vals
|
||
# roll /= 100
|
||
# pitch /= 100
|
||
# yaw /= 100
|
||
# temp = temp / 100
|
||
return f"姿态角 (°): roll={roll:.2f}, pitch={pitch:.2f}, yaw={yaw:.2f}, 温度={temp:.2f}℃"
|
||
elif packet_type == 0x54: # 磁力计，单位uT
|
||
mx, my, mz, temp = vals
|
||
temp = temp / 100
|
||
#return f"磁力计 (uT): x={mx}, y={my}, z={mz}, 温度={temp:.2f}℃"
|
||
elif packet_type == 0x56: # 气压，单位Pa
|
||
p1, p2, p3, temp = vals
|
||
pressure = ((p1 & 0xFFFF) | ((p2 & 0xFFFF) << 16)) / 100 # 简单合成，大部分IMU气压是3字节，这里简单示范
|
||
temp = temp / 100
|
||
# return f"气压 (Pa): pressure={pressure:.2f}, 温度={temp:.2f}℃"
|
||
else:
|
||
return f"未知包类型: {packet_type}"
|
||
|
||
def read_imu(port='COM6', baudrate=9600):
|
||
ser = serial.Serial(port, baudrate, timeout=1)
|
||
buffer = bytearray()
|
||
|
||
try:
|
||
while True:
|
||
bytes_waiting = ser.in_waiting
|
||
if bytes_waiting:
|
||
data = ser.read(bytes_waiting)
|
||
buffer.extend(data)
|
||
|
||
# 解析buffer中所有完整包
|
||
while len(buffer) >= 11:
|
||
if buffer[0] != 0x55:
|
||
buffer.pop(0)
|
||
continue
|
||
packet = buffer[:11]
|
||
result = parse_packet(packet)
|
||
if result:
|
||
print(result)
|
||
buffer = buffer[11:]
|
||
time.sleep(0.01)
|
||
except KeyboardInterrupt:
|
||
print("程序终止")
|
||
finally:
|
||
ser.close()
|
||
|
||
if __name__ == "__main__":
|
||
read_imu(port='COM8', baudrate=9600)
|