增加通讯协议

2026-10-14 09:59:42 +08:00
parent 32878a9a80
commit 950e9e6c9e
102 changed files with 5538 additions and 2679 deletions
@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+"""遥控器帧模拟发送器（联调用，非工程代码）。
+
+按 24 字节协议向 UI 的 net/rc_port（默认 9002）周期发送遥控器报文。
+用法示例：
+  python rc_sender.py --mode drive --gear 4096        # 行车模式，档位推到"前"
+  python rc_sender.py --mode arm --posx 4096          # 机械臂模式（自动带上机械臂使能）
+  python rc_sender.py --estop                         # bit4=1，触发急停
+  python rc_sender.py --battery 42                    # 电量显示 42%
+  python rc_sender.py --roll 4096 --pitch 0           # 右摇杆两轴反向
+  python rc_sender.py --bad-checksum                  # 校验和故意错，应被丢弃
+"""
+import argparse
+import socket
+import struct
+import time
+
+MID = 2048
+
+
+def build_frame(a):
+    flags = 0
+    if a.mode == "drive":
+        flags |= 0x01                      # bit0 行车模式
+        if not a.no_chassis_enable:
+            flags |= 0x08                  # bit3 底盘使能
+    else:
+        flags |= 0x02 | 0x04               # bit1 机械臂模式须与 bit2 使能同时置 1
+    if a.estop:
+        flags |= 0x10                      # bit4 急停
+
+    f = bytearray(24)
+    f[0] = 0x11                            # 帧头
+    f[1] = flags
+    axes = (a.gear, a.steer, a.posz, a.posx, a.posy, a.roll, a.pitch, a.yaw)
+    for i, v in enumerate(axes):
+        struct.pack_into("<H", f, 3 + i * 2, v)   # BYTE3 起，小端 int16
+    f[19] = max(0, min(100, a.battery))
+    f[22] = 0x22                           # 帧尾
+    f[23] = sum(f[0:23]) & 0xFF            # 和校验
+    if a.bad_checksum:
+        f[23] ^= 0xFF
+    return bytes(f)
+
+
+def main():
+    p = argparse.ArgumentParser()
+    p.add_argument("--host", default="127.0.0.1")
+    p.add_argument("--port", type=int, default=9002)
+    p.add_argument("--mode", choices=["drive", "arm"], default="drive")
+    for name in ("gear", "steer", "posz", "posx", "posy", "roll", "pitch", "yaw"):
+        p.add_argument("--" + name, type=int, default=MID)
+    p.add_argument("--battery", type=int, default=88)
+    p.add_argument("--estop", action="store_true")
+    p.add_argument("--no-chassis-enable", action="store_true")
+    p.add_argument("--bad-checksum", action="store_true")
+    p.add_argument("--hz", type=float, default=10.0, help="默认 10Hz，即协议要求的 100ms")
+    p.add_argument("--count", type=int, default=0, help="0=一直发")
+    a = p.parse_args()
+
+    frame = build_frame(a)
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    print("→ %s:%d  mode=%s flags=0x%02X checksum=0x%02X bytes=%d rate=%gHz"
+          % (a.host, a.port, a.mode, frame[1], frame[23], len(frame), a.hz))
+
+    period = 1.0 / a.hz
+    sent = 0
+    try:
+        while a.count == 0 or sent < a.count:
+            sock.sendto(frame, (a.host, a.port))
+            sent += 1
+            time.sleep(period)
+    except KeyboardInterrupt:
+        pass
+    print("已发送 %d 帧" % sent)
+
+
+if __name__ == "__main__":
+    main()