90 lines
2.8 KiB
Python
90 lines
2.8 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 /= 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 /= 100
|
|
# return f"角速度 (°/s): x={wx:.2f}, y={wy:.2f}, z={wz:.2f}, 温度={temp:.2f}℃"
|
|
|
|
if packet_type == 0x53: # 姿态角
|
|
rxl, pitchl, yawl, temp = vals # 注意这里 vals 已经是有符号整数
|
|
# 使用第一段代码里的比例系数
|
|
k_angle = 180.0
|
|
roll = rxl / 32768.0 * k_angle
|
|
pitch = pitchl / 32768.0 * k_angle
|
|
yaw = yawl / 32768.0 * k_angle
|
|
|
|
temp /= 100 # 温度依然是 0.01°C
|
|
|
|
return f"姿态角 (°): roll={roll:.2f}, pitch={pitch:.2f}, yaw={yaw:.2f}, 温度={temp:.2f}℃"
|
|
|
|
# elif packet_type == 0x54: # 磁力计 (单位 uT)
|
|
# mx, my, mz, temp = vals
|
|
# 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
|
|
# temp /= 100
|
|
# return f"气压 (Pa): pressure={pressure:.2f}, 温度={temp:.2f}℃"
|
|
|
|
# else:
|
|
# return f"未知包类型: {packet_type:#04x}"
|
|
|
|
def read_imu(port='COM8', 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)
|
|
|
|
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)
|