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)
|