Your Name
108 lines
2.5 KiB
Python
108 lines
2.5 KiB
Python
#!/usr/bin/env python3
|
|
|
|
import struct
|
|
import serial
|
|
|
|
# -----------------------------------------------------------------------------
|
|
# Configuration
|
|
# -----------------------------------------------------------------------------
|
|
|
|
PORT = "/dev/ttyACM1"
|
|
BAUDRATE = 115200
|
|
|
|
COMMAND_PREFIX = 0x69
|
|
COMMAND_ID = 0x00
|
|
|
|
COMMAND_GET_POLAR = 8
|
|
|
|
# -----------------------------------------------------------------------------
|
|
# Build empty payload
|
|
# -----------------------------------------------------------------------------
|
|
|
|
payload = bytearray()
|
|
length = len(payload)
|
|
|
|
# -----------------------------------------------------------------------------
|
|
# Build packet
|
|
# -----------------------------------------------------------------------------
|
|
|
|
packet = bytearray([
|
|
COMMAND_PREFIX,
|
|
length,
|
|
COMMAND_ID,
|
|
COMMAND_GET_POLAR,
|
|
0x00, # CRC placeholder
|
|
])
|
|
|
|
packet += payload
|
|
|
|
# -----------------------------------------------------------------------------
|
|
# Calculate CRC
|
|
# -----------------------------------------------------------------------------
|
|
|
|
crc_sum = 0
|
|
|
|
for i, byte in enumerate(packet):
|
|
if i == 4:
|
|
continue
|
|
crc_sum += byte
|
|
|
|
packet[4] = (0x100 - (crc_sum & 0xFF)) & 0xFF
|
|
|
|
print("Request:")
|
|
print(packet.hex(" "))
|
|
|
|
# -----------------------------------------------------------------------------
|
|
# Send request and receive response
|
|
# -----------------------------------------------------------------------------
|
|
|
|
print("TX bytearray:")
|
|
print(packet)
|
|
|
|
print("TX hex:")
|
|
print(packet.hex(" "))
|
|
|
|
with serial.Serial(PORT, BAUDRATE, timeout=1) as ser:
|
|
ser.write(packet)
|
|
ser.flush()
|
|
|
|
# Read header
|
|
header = ser.read(5)
|
|
|
|
if len(header) != 5:
|
|
raise RuntimeError("Timeout waiting for response header")
|
|
|
|
prefix, length, cmd_id, command, crc = header
|
|
|
|
# Read payload
|
|
payload = ser.read(length)
|
|
|
|
if len(payload) != length:
|
|
raise RuntimeError("Timeout waiting for response payload")
|
|
|
|
# Combine header + payload into a single bytearray
|
|
response = bytearray(header)
|
|
response.extend(payload)
|
|
|
|
print("\nRX bytearray:")
|
|
print(response)
|
|
|
|
print("RX hex:")
|
|
print(response.hex(" "))
|
|
|
|
print(f"\nPrefix : 0x{prefix:02X}")
|
|
print(f"Length : {length}")
|
|
print(f"Cmd ID : {cmd_id}")
|
|
print(f"Command: {command}")
|
|
print(f"CRC : 0x{crc:02X}")
|
|
|
|
if command != COMMAND_GET_POLAR:
|
|
raise RuntimeError(f"Unexpected response command {command}")
|
|
|
|
if length != 8:
|
|
raise RuntimeError(f"Expected 8-byte payload, got {length}")
|
|
|
|
theta, r = struct.unpack("<ff", payload)
|
|
|
|
print(f"\nTheta = {theta:.3f}")
|
|
print(f"R = {r:.3f}") |