feat: add DEBUG mode with host-side PID tuning, motor enable/disable protection
- Add DEBUG mode (parallel to AUTO/MANUAL) for real-time PID parameter tuning - New 0xA8 debug frame: 12 floats for vision/motor angle PID tuning params - DMMotorSetMode: send zero-velocity before disable, clear PID to prevent jumps - motor_enable topic: pub from robot_cmd, sub in gimbal, controls motor on/off - motor_pid topic: pub from robot_cmd, sub in gimbal, updates motor angle PID - Rename DM motor enums (DM_CMD_ENABLE_MODE/DISABLE_MODE, DM_Motor_Ctrl_Mode_e) - SwitchToAuto/Manual: re-enable motors when switching back from DEBUG mode - Static-allocate enable_msg/pid_tune in GimbalTask to avoid stack churn - Add docs/host_comm_guide.md with Python Tkinter tuning GUI example - Remove ARCHITECTURE.md
This commit is contained in:
@@ -0,0 +1,244 @@
|
||||
# 上位机通信协议指南
|
||||
|
||||
## 串口配置
|
||||
|
||||
| 参数 | 值 |
|
||||
|---|---|
|
||||
| 波特率 | 115200 |
|
||||
| 数据位 | 8 |
|
||||
| 停止位 | 1 |
|
||||
| 校验位 | None |
|
||||
|
||||
## 帧格式总览
|
||||
|
||||
所有帧通过帧头 (SOF) 区分类型，checksum = 帧头到数据末尾前一字节的所有字节累加和取低 8 位。
|
||||
|
||||
| 帧头 | 长度 | 用途 |
|
||||
|---|---|---|
|
||||
| 0xA5 | 10 | 自动/视觉追踪数据 |
|
||||
| 0xA6 | 11 | 手动控制数据 |
|
||||
| 0xA7 | 3 | 命令 |
|
||||
| 0xA8 | 50 | 调试调参数据 |
|
||||
|
||||
---
|
||||
|
||||
## 1. 自动/视觉追踪数据帧 0xA5 (10 字节)
|
||||
|
||||
**用途**: 视觉识别完成后持续发送跟踪误差。
|
||||
|
||||
```
|
||||
[SOF:0xA5][error_x:4][error_y:4][checksum:1]
|
||||
```
|
||||
|
||||
| 偏移 | 长度 | 类型 | 含义 |
|
||||
|---|---|---|---|
|
||||
| 0 | 1 | uint8 | 帧头 0xA5 |
|
||||
| 1 | 4 | float | 归一化 X 轴误差 |
|
||||
| 5 | 4 | float | 归一化 Y 轴误差 |
|
||||
| 9 | 1 | uint8 | checksum |
|
||||
|
||||
**Python 写入示例**:
|
||||
|
||||
```python
|
||||
import struct
|
||||
|
||||
def send_auto_tracking(ser, error_x: float, error_y: float):
|
||||
data = bytearray()
|
||||
data.append(0xA5)
|
||||
data.extend(struct.pack('<ff', error_x, error_y))
|
||||
checksum = sum(data) & 0xFF
|
||||
data.append(checksum)
|
||||
ser.write(data)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 2. 手动控制数据帧 0xA6 (11 字节)
|
||||
|
||||
**用途**: 手动模式下发送角度目标值。
|
||||
|
||||
```
|
||||
[SOF:0xA6][zero_force:1][yaw_ref:4][pitch_ref:4][checksum:1]
|
||||
```
|
||||
|
||||
| 偏移 | 长度 | 类型 | 含义 |
|
||||
|---|---|---|---|
|
||||
| 0 | 1 | uint8 | 帧头 0xA6 |
|
||||
| 1 | 1 | uint8 | 零力标志 (0=正常, 1=零力) |
|
||||
| 2 | 4 | float | Yaw 角度参考 (degrees) |
|
||||
| 6 | 4 | float | Pitch 角度参考 (degrees) |
|
||||
| 10 | 1 | uint8 | checksum |
|
||||
|
||||
**Python 写入示例**:
|
||||
|
||||
```python
|
||||
def send_manual_control(ser, yaw_deg: float, pitch_deg: float, zero_force: int = 0):
|
||||
data = bytearray()
|
||||
data.append(0xA6)
|
||||
data.append(0 if zero_force else 0)
|
||||
data.extend(struct.pack('<ff', yaw_deg, pitch_deg))
|
||||
checksum = sum(data) & 0xFF
|
||||
data.append(checksum)
|
||||
ser.write(data)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. 命令帧 0xA7 (3 字节)
|
||||
|
||||
**用途**: 切换模式和控制激光。
|
||||
|
||||
```
|
||||
[SOF:0xA7][cmd:1][checksum:1]
|
||||
```
|
||||
|
||||
| cmd 值 | 含义 |
|
||||
|---|---|
|
||||
| 0x01 | 切换到自动/视觉追踪模式 |
|
||||
| 0x02 | 切换到手动模式 |
|
||||
| 0x05 | 切换到调试模式 (失能所有电机) |
|
||||
| 0x03 | 激光开 |
|
||||
| 0x04 | 激光关 |
|
||||
|
||||
**Python 写入示例**:
|
||||
|
||||
```python
|
||||
CMD_AUTO = 0x01
|
||||
CMD_MANUAL = 0x02
|
||||
CMD_LASER_ON = 0x03
|
||||
CMD_LASER_OFF = 0x04
|
||||
CMD_DEBUG = 0x05
|
||||
|
||||
def send_command(ser, cmd: int):
|
||||
data = bytearray([0xA7, cmd])
|
||||
checksum = sum(data) & 0xFF
|
||||
data.append(checksum)
|
||||
ser.write(data)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 4. 调试调参帧 0xA8 (50 字节)
|
||||
|
||||
**用途**: 在调试模式下发送 PID 参数进行调参。发送后固件会立即生效。
|
||||
|
||||
```
|
||||
[SOF:0xA8]
|
||||
[vision_yaw_Kp:4][vision_yaw_Ki:4][vision_yaw_Kd:4]
|
||||
[vision_pitch_Kp:4][vision_pitch_Ki:4][vision_pitch_Kd:4]
|
||||
[motor_yaw_Kp:4][motor_yaw_Ki:4][motor_yaw_Kd:4]
|
||||
[motor_pitch_Kp:4][motor_pitch_Ki:4][motor_pitch_Kd:4]
|
||||
[checksum:1]
|
||||
```
|
||||
|
||||
| 偏移 | 长度 | 类型 | 含义 |
|
||||
|---|---|---|---|
|
||||
| 0 | 1 | uint8 | 帧头 0xA8 |
|
||||
| 1 | 4 | float | 视觉环 Yaw Kp |
|
||||
| 5 | 4 | float | 视觉环 Yaw Ki |
|
||||
| 9 | 4 | float | 视觉环 Yaw Kd |
|
||||
| 13 | 4 | float | 视觉环 Pitch Kp |
|
||||
| 17 | 4 | float | 视觉环 Pitch Ki |
|
||||
| 21 | 4 | float | 视觉环 Pitch Kd |
|
||||
| 25 | 4 | float | 电机角度环 Yaw Kp |
|
||||
| 29 | 4 | float | 电机角度环 Yaw Ki |
|
||||
| 33 | 4 | float | 电机角度环 Yaw Kd |
|
||||
| 37 | 4 | float | 电机角度环 Pitch Kp |
|
||||
| 41 | 4 | float | 电机角度环 Pitch Ki |
|
||||
| 45 | 4 | float | 电机角度环 Pitch Kd |
|
||||
| 49 | 1 | uint8 | checksum |
|
||||
|
||||
**Python 写入示例**:
|
||||
|
||||
```python
|
||||
def send_debug_tune(ser,
|
||||
v_yaw_kp, v_yaw_ki, v_yaw_kd,
|
||||
v_pitch_kp, v_pitch_ki, v_pitch_kd,
|
||||
m_yaw_kp, m_yaw_ki, m_yaw_kd,
|
||||
m_pitch_kp, m_pitch_ki, m_pitch_kd):
|
||||
data = bytearray()
|
||||
data.append(0xA8)
|
||||
data.extend(struct.pack('<fff', v_yaw_kp, v_yaw_ki, v_yaw_kd))
|
||||
data.extend(struct.pack('<fff', v_pitch_kp, v_pitch_ki, v_pitch_kd))
|
||||
data.extend(struct.pack('<fff', m_yaw_kp, m_yaw_ki, m_yaw_kd))
|
||||
data.extend(struct.pack('<fff', m_pitch_kp, m_pitch_ki, m_pitch_kd))
|
||||
checksum = sum(data) & 0xFF
|
||||
data.append(checksum)
|
||||
ser.write(data)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 调试流程
|
||||
|
||||
1. 上位机发送命令帧 (0xA7, cmd=0x05) 切换到调试模式
|
||||
2. 固件收到后失能 yaw 和 pitch 电机，日志指示进入调试模式
|
||||
3. 通过 0xA8 帧写入 PID 参数，参数会**实时生效**：
|
||||
- 视觉环 PID → 直接写入选中的视觉 PID 实例
|
||||
- 电机角度环 PID → 通过话题推送到 gimbal 更新电机角度 PID
|
||||
4. 调试完毕后，发送命令帧 (0xA7, cmd=0x02 手动 或 0x01 自动) 切出调试模式，电机会自动使能
|
||||
|
||||
---
|
||||
|
||||
## Tkinter 界面建议
|
||||
|
||||
```python
|
||||
import tkinter as tk
|
||||
import tkinter.ttk as ttk
|
||||
import serial
|
||||
import struct
|
||||
import threading
|
||||
|
||||
class TunerApp:
|
||||
def __init__(self, root):
|
||||
self.ser = serial.Serial('COM3', 115200, timeout=0.01)
|
||||
|
||||
# 视觉环 PID
|
||||
self.vision_frame = ttk.LabelFrame(root, text="视觉环 PID")
|
||||
self.v_yaw = self._make_pid_row(self.vision_frame, "Yaw", 0, (1.5, 3.0, 0.07))
|
||||
self.v_pitch = self._make_pid_row(self.vision_frame, "Pitch", 1, (1.0, 2.0, 0.05))
|
||||
|
||||
# 电机角度环 PID
|
||||
self.motor_frame = ttk.LabelFrame(root, text="电机角度环 PID")
|
||||
self.m_yaw = self._make_pid_row(self.motor_frame, "Yaw", 2, (30, 80, 0.2))
|
||||
self.m_pitch = self._make_pid_row(self.motor_frame, "Pitch", 3, (20, 50, 0.1))
|
||||
|
||||
ttk.Button(root, text="发送调参", command=self.send_tune).pack()
|
||||
|
||||
def _make_pid_row(self, parent, label, row, default):
|
||||
frame = ttk.Frame(parent)
|
||||
ttk.Label(frame, text=label).grid(row=0, column=0)
|
||||
kp = tk.DoubleVar(value=default[0])
|
||||
ki = tk.DoubleVar(value=default[1])
|
||||
kd = tk.DoubleVar(value=default[2])
|
||||
ttk.Entry(frame, textvariable=kp, width=8).grid(row=0, column=1)
|
||||
ttk.Entry(frame, textvariable=ki, width=8).grid(row=0, column=2)
|
||||
ttk.Entry(frame, textvariable=kd, width=8).grid(row=0, column=3)
|
||||
frame.pack()
|
||||
return (kp, ki, kd)
|
||||
|
||||
def send_tune(self):
|
||||
def get(v): return v[0].get(), v[1].get(), v[2].get()
|
||||
vy_kp, vy_ki, vy_kd = get(self.v_yaw)
|
||||
vp_kp, vp_ki, vp_kd = get(self.v_pitch)
|
||||
my_kp, my_ki, my_kd = get(self.m_yaw)
|
||||
mp_kp, mp_ki, mp_kd = get(self.m_pitch)
|
||||
send_debug_tune(self.ser,
|
||||
vy_kp, vy_ki, vy_kd,
|
||||
vp_kp, vp_ki, vp_kd,
|
||||
my_kp, my_ki, my_kd,
|
||||
mp_kp, mp_ki, mp_kd)
|
||||
|
||||
root = tk.Tk()
|
||||
app = TunerApp(root)
|
||||
root.mainloop()
|
||||
```
|
||||
|
||||
## 校验和计算
|
||||
|
||||
```python
|
||||
def calc_checksum(data: bytes) -> int:
|
||||
return sum(data) & 0xFF # 所有字节累加取低 8 位
|
||||
```
|
||||
|
||||
校验和覆盖范围：**帧头到数据末尾前一字节**的全部字节。接收端在收到数据后也应计算并比对校验和。
|
||||
Reference in New Issue
Block a user