76 lines
3.4 KiB
Python
76 lines
3.4 KiB
Python
import serial
|
|
import time
|
|
import struct
|
|
|
|
def configure_module(ser):
|
|
# Comando para configurar o módulo para enviar todos os pacotes de dados
|
|
comando_configurar = bytes([0xFF, 0xAA, 0x02, 0x1F, 0x00]) # Habilita pacotes 0x50, 0x51, 0x52, 0x53, 0x54, 0x59
|
|
ser.write(comando_configurar)
|
|
time.sleep(0.1) # Pequena pausa para garantir que o comando seja processado
|
|
|
|
def request_data(ser):
|
|
# Comando para requisitar dados
|
|
comando_requisitar = bytes([0xFF, 0xAA, 0x02, 0x1F, 0x00])
|
|
ser.write(comando_requisitar)
|
|
|
|
def read_data(ser):
|
|
while True:
|
|
request_data(ser)
|
|
time.sleep(1)
|
|
if ser.in_waiting > 0:
|
|
data = ser.read(ser.in_waiting)
|
|
process_data(data)
|
|
|
|
def process_data(data):
|
|
index = 0
|
|
while index < len(data):
|
|
header = data[index]
|
|
if header == 0x55: # Supondo que 0x55 é o cabeçalho
|
|
packet_type = data[index + 1]
|
|
if packet_type == 0x50: # Pacote de tempo
|
|
time_data = struct.unpack('<BBBBBBBBB', data[index + 2:index + 11])
|
|
ms = (time_data[7] << 8) | time_data[8]
|
|
print(f"Time: 20{time_data[0]:02}-{time_data[1]:02}-{time_data[2]:02} {time_data[3]:02}:{time_data[4]:02}:{time_data[5]:02}.{ms:03}")
|
|
elif packet_type == 0x51: # Pacote de aceleração
|
|
accel = struct.unpack('<hhhH', data[index + 2:index + 10])
|
|
ax = accel[0] / 32768.0 * 16
|
|
ay = accel[1] / 32768.0 * 16
|
|
az = accel[2] / 32768.0 * 16
|
|
temp = accel[3] / 100.0
|
|
print(f"Acceleration: X={ax:.2f}g, Y={ay:.2f}g, Z={az:.2f}g, Temp={temp:.2f}°C")
|
|
elif packet_type == 0x52: # Pacote de velocidade angular
|
|
gyro = struct.unpack('<hhhH', data[index + 2:index + 10])
|
|
wx = gyro[0] / 32768.0 * 2000
|
|
wy = gyro[1] / 32768.0 * 2000
|
|
wz = gyro[2] / 32768.0 * 2000
|
|
voltage = gyro[3] / 100.0
|
|
print(f"Gyro: X={wx:.2f}°/s, Y={wy:.2f}°/s, Z={wz:.2f}°/s, Voltage={voltage:.2f}V")
|
|
elif packet_type == 0x53: # Pacote de ângulo
|
|
angle = struct.unpack('<hhhH', data[index + 2:index + 10])
|
|
roll = angle[0] / 32768.0 * 180
|
|
pitch = angle[1] / 32768.0 * 180
|
|
yaw = angle[2] / 32768.0 * 180
|
|
version = angle[3]
|
|
print(f"Angle: Roll={roll:.2f}°, Pitch={pitch:.2f}°, Yaw={yaw:.2f}°, Version={version}")
|
|
elif packet_type == 0x54: # Pacote magnético
|
|
magnetic = struct.unpack('<hhhH', data[index + 2:index + 10])
|
|
hx = magnetic[0]
|
|
hy = magnetic[1]
|
|
hz = magnetic[2]
|
|
temp = magnetic[3] / 100.0
|
|
print(f"Magnetic: X={hx}, Y={hy}, Z={hz}, Temp={temp:.2f}°C")
|
|
elif packet_type == 0x59: # Pacote de quaternions
|
|
quaternion = struct.unpack('<hhhh', data[index + 2:index + 10])
|
|
q0 = quaternion[0] / 32768.0
|
|
q1 = quaternion[1] / 32768.0
|
|
q2 = quaternion[2] / 32768.0
|
|
q3 = quaternion[3] / 32768.0
|
|
print(f"Quaternion: Q0={q0:.2f}, Q1={q1:.2f}, Q2={q2:.2f}, Q3={q3:.2f}")
|
|
index += 11 # Avançar para o próximo pacote
|
|
else:
|
|
index += 1
|
|
|
|
if __name__ == "__main__":
|
|
ser = serial.Serial('COM3', 115200, timeout=1)
|
|
configure_module(ser)
|
|
read_data(ser) |