80 lines
2.9 KiB
Python
80 lines
2.9 KiB
Python
#!/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()
|