ShunFang-cart/ESP
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

real field test

Browse Source
This commit is contained in:
Brunsmeier
2026-06-18 16:55:30 +08:00
commit 55aea83cbe
5 changed files with 671 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,4 @@
{
"python-envs.defaultEnvManager": "ms-python.python:system",
"python-envs.defaultPackageManager": "ms-python.python:pip"
}

344
main.py Normal file
View File

@ -0,0 +1,344 @@
from machine import Pin, UART
import struct
import time
import sys
import select
# ============================================================
# ESP32 + IMU + USB binary bridge
#
# Wiring:
# IMU TX yellow -> ESP32 GPIO16 / RX2
# IMU RX green -> ESP32 GPIO17 / TX2
# IMU GND -> ESP32 GND
#
# UART2: IMU
# USB stdout/stdin: PC
#
# Important:
# - Output uses sys.stdout, because this is the stable path.
# - Command input uses sys.stdin, ASCII only: o / c / s.
# - First test with USE_REAL_DOUT = False.
# ============================================================
# ---------------- PIN CONFIG ----------------
IMU_RX_PIN = 16
IMU_TX_PIN = 17
DIN_PINS = (25, 26, 27, 32)
DOUT_PINS = (18, 19)
digital_inputs = [Pin(pin, Pin.IN, Pin.PULL_UP) for pin in DIN_PINS]
digital_outputs = [Pin(pin, Pin.OUT, value=0) for pin in DOUT_PINS]
# ---------------- SERIAL CONFIG ----------------
# PC side uses MicroPython stdout/stdin.
# PC reader should use:
# python3 pc_reader.py /dev/ttyUSB0 --baud 115200
PC_BAUD_NOTE = 115200
# IMU side confirmed working.
IMU_BAUD = 921600
SEND_PERIOD_MS = 20 # 50 Hz
STARTUP_DELAY_MS = 3000 # quiet window after reset
# ---------------- IMU FRAME CONFIG ----------------
FRAME_LEN = 11
HEADER = 0x55
TYPE_ACC = 0x51
TYPE_GYRO = 0x52
TYPE_ANGLE = 0x53
# raw_values:
# acc x y z, gyro x y z, angle x y z, temperature
raw_values = [0] * 10
# ---------------- DOUT TEST MODE ----------------
# False:
# Only update software DOUT state in outgoing packet.
# GPIO18/GPIO19 will NOT really change.
#
# True:
# Actually drive GPIO18/GPIO19.
#
# Start with False. If o/c/s changes DOUT in pc_reader output,
# then set it to True and test real hardware.
USE_REAL_DOUT = True
# bit0 = DOUT0 / GPIO18
# bit1 = DOUT1 / GPIO19
dout_shadow = 0
# ---------------- BUFFERS ----------------
rx_buffer = bytearray(2048)
write_idx = 0
read_idx = 0
# ---------------- USB STDOUT / STDIN ----------------
try:
usb_out = sys.stdout.buffer
except AttributeError:
usb_out = sys.stdout
poller = select.poll()
poller.register(sys.stdin, select.POLLIN)
# ============================================================
# DOUT / gripper
# ============================================================
def clear_douts():
global dout_shadow
dout_shadow = 0
for pin in digital_outputs:
pin.value(0)
def set_gripper(mode):
global dout_shadow
# mode:
# 0 = STOP
# 1 = OPEN
# 2 = CLOSE
#
# DOUT[0] = GPIO18
# DOUT[1] = GPIO19
if mode == 1:
# OPEN -> DOUT=[0, 1]
dout_shadow = 0b10
elif mode == 2:
# CLOSE -> DOUT=[1, 0]
dout_shadow = 0b01
else:
# STOP -> DOUT=[0, 0]
dout_shadow = 0b00
# Shadow test mode: do not touch real GPIO pins.
if not USE_REAL_DOUT:
return
# Real output mode.
digital_outputs[0].value(0)
digital_outputs[1].value(0)
time.sleep_ms(20)
if mode == 1:
# OPEN
digital_outputs[1].value(1)
elif mode == 2:
# CLOSE
digital_outputs[0].value(1)
# mode 0 keeps both LOW
# ============================================================
# IMU UART
# ============================================================
def open_imu_uart():
uart = UART(
2,
baudrate=IMU_BAUD,
bits=8,
parity=None,
stop=1,
rx=IMU_RX_PIN,
tx=IMU_TX_PIN,
timeout=0,
rxbuf=4096,
)
time.sleep_ms(300)
# Flush startup junk.
while uart.any():
uart.read()
return uart
# ============================================================
# IMU parser
# ============================================================
def update_raw_values(frame):
frame_type = frame[1]
x, y, z, extra = struct.unpack_from("<hhhh", frame, 2)
if frame_type == TYPE_ACC:
raw_values[0:3] = (x, y, z)
raw_values[9] = extra
elif frame_type == TYPE_GYRO:
raw_values[3:6] = (x, y, z)
elif frame_type == TYPE_ANGLE:
raw_values[6:9] = (x, y, z)
return True
return False
def poll_imu(uart):
global write_idx, read_idx
count = uart.any()
if count:
data = uart.read(count)
if data:
for b in data:
rx_buffer[write_idx] = b
write_idx = (write_idx + 1) % len(rx_buffer)
# Buffer full: drop oldest byte.
if write_idx == read_idx:
read_idx = (read_idx + 1) % len(rx_buffer)
updated = False
while True:
available = (write_idx - read_idx) % len(rx_buffer)
if available < FRAME_LEN:
break
# Resync to 0x55.
if rx_buffer[read_idx] != HEADER:
read_idx = (read_idx + 1) % len(rx_buffer)
continue
frame = bytearray(FRAME_LEN)
for i in range(FRAME_LEN):
frame[i] = rx_buffer[(read_idx + i) % len(rx_buffer)]
# Checksum.
if (sum(frame[:10]) & 0xFF) != frame[10]:
read_idx = (read_idx + 1) % len(rx_buffer)
continue
read_idx = (read_idx + FRAME_LEN) % len(rx_buffer)
if update_raw_values(frame):
updated = True
return updated
# ============================================================
# Packet to PC
# ============================================================
def make_packet():
din_mask = 0
for index, pin in enumerate(digital_inputs):
din_mask |= pin.value() << index
if USE_REAL_DOUT:
dout_mask = 0
for index, pin in enumerate(digital_outputs):
dout_mask |= pin.value() << index
else:
dout_mask = dout_shadow
# Packet layout:
# A5 5A | version | DIN | DOUT | ticks_ms | 10 signed int16 | checksum
packet = struct.pack(
"<2sBBBI10h",
b"\xA5\x5A",
2,
din_mask,
dout_mask,
time.ticks_ms() & 0xFFFFFFFF,
*raw_values,
)
return packet + bytes((sum(packet) & 0xFF,))
def usb_write(data):
usb_out.write(data)
# ============================================================
# USB command parser
# ============================================================
def poll_usb_commands():
# ASCII only.
# pc_reader.py should send:
# b"o" = OPEN
# b"c" = CLOSE
# b"s" = STOP
#
# Do NOT use C3 3C binary command here.
for _ in range(8):
if not poller.poll(0):
break
ch = sys.stdin.read(1)
if not ch:
break
if ch == "o" or ch == "O":
set_gripper(1)
elif ch == "c" or ch == "C":
set_gripper(2)
elif ch == "s" or ch == "S":
set_gripper(0)
# Ignore Enter/newline and other characters.
# ============================================================
# Main
# ============================================================
clear_douts()
# Quiet window after reset.
# During this period there is no binary output.
time.sleep_ms(STARTUP_DELAY_MS)
imu_uart = open_imu_uart()
last_send_ms = time.ticks_ms()
while True:
poll_usb_commands()
poll_imu(imu_uart)
now = time.ticks_ms()
# Fixed 50 Hz output.
if time.ticks_diff(now, last_send_ms) >= SEND_PERIOD_MS:
usb_write(make_packet())
last_send_ms = now
time.sleep_ms(0)

121
pc_reader.py Normal file
View File

@ -0,0 +1,121 @@
import argparse
import struct
import threading
import serial
SYNC = b"\xA5\x5A"
PACKET_FORMAT = "<2sBBBI10hB"
PACKET_SIZE = struct.calcsize(PACKET_FORMAT)
def decode_packet(packet):
fields = struct.unpack(PACKET_FORMAT, packet)
_, version, din_mask, dout_mask, time_ms, *rest = fields
raw = rest[:-1]
acc_scale = 16.0 / 32768.0
gyro_scale = 2000.0 / 32768.0
angle_scale = 180.0 / 32768.0
return {
"version": version,
"t_ms": time_ms,
"din": [(din_mask >> bit) & 1 for bit in range(4)],
"dout": [(dout_mask >> bit) & 1 for bit in range(2)],
"acc_g": [round(value * acc_scale, 5) for value in raw[0:3]],
"gyro_dps": [round(value * gyro_scale, 4) for value in raw[3:6]],
"angle_deg": [round(value * angle_scale, 4) for value in raw[6:9]],
"temp_c": round(raw[9] / 100.0, 2),
}
def send_gripper_command(ser, mode):
if mode == 1:
ser.write(b"o")
elif mode == 2:
ser.write(b"c")
else:
ser.write(b"s")
def keyboard_control(ser):
print("Commands: o=OPEN, c=CLOSE, s=STOP")
while True:
text = input().strip().lower()
if text == "o":
send_gripper_command(ser, 1)
elif text == "c":
send_gripper_command(ser, 2)
elif text == "s":
send_gripper_command(ser, 0)
else:
print("Unknown command. Use o, c, or s.")
def main():
parser = argparse.ArgumentParser()
parser.add_argument("port", help="ESP32 serial port, for example COM5")
parser.add_argument("--baud", type=int, default=230400)
args = parser.parse_args()
buffer = bytearray()
packet_count = 0
last_time_ms = None
with serial.Serial(args.port, args.baud, timeout=1) as ser:
print("Reading {} at {} baud".format(args.port, args.baud))
threading.Thread(
target=keyboard_control,
args=(ser,),
daemon=True,
).start()
while True:
chunk = ser.read(ser.in_waiting or 1)
if chunk:
buffer.extend(chunk)
while len(buffer) >= PACKET_SIZE:
sync_index = buffer.find(SYNC)
if sync_index < 0:
del buffer[:-1]
break
if sync_index:
del buffer[:sync_index]
if len(buffer) < PACKET_SIZE:
break
packet = bytes(buffer[:PACKET_SIZE])
if (sum(packet[:-1]) & 0xFF) != packet[-1]:
del buffer[0]
continue
del buffer[:PACKET_SIZE]
data = decode_packet(packet)
packet_count += 1
if last_time_ms is None:
frequency = 0.0
else:
delta = (data["t_ms"] - last_time_ms) & 0xFFFFFFFF
frequency = 1000.0 / delta if delta else 0.0
last_time_ms = data["t_ms"]
print(
"#{:8d} {:6.1f}Hz DIN={} DOUT={} "
"ACC={} GYRO={} ANGLE={}".format(
packet_count,
frequency,
data["din"],
data["dout"],
data["acc_g"],
data["gyro_dps"],
data["angle_deg"],
)
)
if __name__ == "__main__":
main()

135
rcd.txt Normal file
View File

@ -0,0 +1,135 @@
792 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00195, -0.00781, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 793 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00244, -0.00781, -1.01025] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 794 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.0083, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 795 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.0083, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 796 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 797 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00195, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 798 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00244, -0.0083, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 799 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.0083, -1.01025] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.1044, -28.9655]
# 800 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00195, -0.00732, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.1044, -28.9655]
# 801 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00195, -0.00879, -1.01025] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.1044, -28.9655]
# 802 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.00732, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 803 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00244, -0.0083, -1.01025] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 804 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00195, -0.0083, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 805 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00195, -0.0083, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 806 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00098, -0.00781, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 807 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.00732, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 808 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.00781, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 809 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.0083, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 810 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00244, -0.0083, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 811 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.00781, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 812 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00195, -0.00879, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 813 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 814 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.00781, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 815 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00195, -0.00781, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 816 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00195, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 817 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.00928, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 818 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.00879, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 819 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00195, -0.0083, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 820 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 821 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00098, -0.00732, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 822 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00098, -0.00879, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 823 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00195, -0.0083, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 824 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00244, -0.0083, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 825 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00146, -0.0083, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 826 50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[-0.00098, -0.0083, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 827 40.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.01172] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 828 52.6Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00098, -0.00879, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 829 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.00781, -1.0127] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 830 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00928, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 831 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00879, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 832 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00098, -0.00732, -1.00537] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 833 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00098, -0.00879, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 834 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 835 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00879, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 836 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 837 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 838 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 839 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00781, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 840 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 841 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00879, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 842 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00781, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 843 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00879, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 844 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.00684] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 845 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.0083, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 846 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.0083, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 847 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00879, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 848 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00293, -0.00928, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 849 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00879, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 850 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00879, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 851 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.0083, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 852 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5221, 0.1044, -28.9655]
# 853 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5221, 0.1044, -28.9655]
# 854 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5221, 0.1044, -28.9655]
# 855 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5221, 0.1044, -28.9655]
# 856 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5221, 0.1044, -28.9655]
# 857 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00635] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5221, 0.1044, -28.9655]
# 858 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00879, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.1044, -28.9655]
# 859 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.1044, -28.9655]
# 860 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00049, -0.0083, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.1044, -28.9655]
# 861 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00732, -1.01074] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 862 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 863 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00098, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 864 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.00879, -1.00684] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 865 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.0083, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 866 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00537] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 867 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00879, -1.01123] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 868 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00879, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 869 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00732, -1.01025] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5276, 0.0989, -28.9655]
# 870 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 871 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.00928, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 872 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.00684] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 873 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 874 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 875 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 876 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00098, -0.00879, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 877 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 878 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 879 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.00781, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 880 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 881 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 882 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 883 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00879, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 884 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 885 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00732, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 886 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 887 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.1044, -28.9655]
# 888 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00781, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.1044, -28.9655]
# 889 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.1044, -28.9655]
# 890 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.00879, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.1044, -28.9655]
# 891 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00732, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.1044, -28.9655]
# 892 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.1044, -28.9655]
# 893 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00879, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 894 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.0083, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 895 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 896 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00781, -1.01074] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 897 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.00781, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 898 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.00977] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 899 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00732, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 900 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 901 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00098, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 902 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00879, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 903 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 904 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 905 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00732, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 906 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 907 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 908 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00732, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 909 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00879, -1.00732] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 910 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.0127] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 911 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00293, -0.00879, -1.00537] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.0989, -28.9655]
# 912 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.01172] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 913 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00781, -1.00684] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 914 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 915 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.00879, -1.00928] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 916 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00879, -1.00635] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 917 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00244, -0.00781, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.1044, -28.9655]
# 918 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.00732, -1.00684] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.1044, -28.9655]
# 919 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00781, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 920 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00732, -1.00879] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 921 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00879, -1.00781] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 922 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.01074] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 923 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.0083, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5386, 0.1044, -28.9655]
# 924 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00195, -0.0083, -1.0083] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]
# 925 50.0Hz DIN=[0, 1, 0, 0] DOUT=[0, 0] ACC=[-0.00146, -0.00781, -1.00635] GYRO=[0.0, 0.0, 0.0] ANGLE=[-179.5331, 0.0989, -28.9655]

67
test.py Normal file
View File

@ -0,0 +1,67 @@
from machine import UART, Pin
import time
CANDIDATES = [4, 12, 13, 14, 16, 17, 21, 22, 23, 25, 26, 27, 32, 33]
BAUDRATES = [921600]
HEADER = 0x55 # IMU WIT frame header
def test_uart(rx, tx, baud):
try:
uart = UART(
2,
baudrate=baud,
rx=Pin(rx),
tx=Pin(tx),
timeout=100,
rxbuf=1024
)
time.sleep(0.1)
start = time.ticks_ms()
samples = []
while time.ticks_diff(time.ticks_ms(), start) < 1000:
if uart.any():
data = uart.read()
if data:
samples.append(data)
uart.deinit()
# 合并所有数据
if samples:
merged = b''.join(samples)
print(f"\nRX={rx}, TX={tx}, BAUD={baud}")
print("RAW HEX:", merged[:80]) # 前80字节
print("HEX:", merged[:80].hex())
return merged
else:
return b""
except Exception as e:
return b""
print("=== UART PIN SCAN START ===")
best = (0, None, None, None)
for rx in CANDIDATES:
for tx in CANDIDATES:
if rx == tx:
continue
for baud in BAUDRATES:
c, t = test_uart(rx, tx, baud)
print(f"RX={rx}, TX={tx}, BAUD={baud} -> 0x55={c}, bytes={t}")
if c > best[0]:
best = (c, rx, tx, baud)
print("\n=== BEST MATCH ===")
print(best)