AimBot/Gimbal
14
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor: replace vision error_x/y with measure/ref in 0xA5 frame

Browse Source 
Change 0xA5 frame from 2-float (error_x, error_y) to 4-float
(measure_x, ref_x, measure_y, ref_y) so PIDCalculate receives
actual measurement and reference values internally computing error.
This commit is contained in:
李傲琦
2026-06-04 13:55:40 +08:00
parent fd2386d33a
commit c0f43940bd
6 changed files with 85 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+7 -7
View File
@@ -67,7 +67,7 @@ main()
┌──────────────────────────────────────────────────────┐ ┌──────────────────────────────────────────────────────┐
│ application/cmd/robot_cmd.c (外环, 500Hz) │ │ application/cmd/robot_cmd.c (外环, 500Hz) │
│ │ │ │
│ AUTO: 上位机 0xA5 帧 → error_x/y → KF → vision PID│ │ AUTO: 上位机 0xA5 帧 → measure_x/y, ref_x/y → KF → vision PID│
│ → 角度增量 → 累计 angle ref │ │ → 角度增量 → 累计 angle ref │
│ MANUAL: 上位机 0xA6 帧 → yaw/pitch 角速度 → 角度增量积分 │ │ MANUAL: 上位机 0xA6 帧 → yaw/pitch 角速度 → 角度增量积分 │
│ DEBUG: 上位机 0xA8 帧 → 实时更新 PID 参数 │ │ DEBUG: 上位机 0xA8 帧 → 实时更新 PID 参数 │
@@ -107,7 +107,7 @@ main()
| 帧头 | 长度 | 用途 | | 帧头 | 长度 | 用途 |
|------|------|------| |------|------|------|
| 0xA5 | 10 | 自动/视觉追踪数据帧 (error_x, error_y) | | 0xA5 | 18 | 自动/视觉追踪数据帧 (measure_x, ref_x, measure_y, ref_y) |
| 0xA6 | 10 | 手动控制数据帧 (yaw_velocity, pitch_velocity) | | 0xA6 | 10 | 手动控制数据帧 (yaw_velocity, pitch_velocity) |
| 0xA7 | 3 | 命令帧 (模式切换, 激光) | | 0xA7 | 3 | 命令帧 (模式切换, 激光) |
| 0xA8 | 50 | 调试调参帧 (12 个 float PID 参数) | | 0xA8 | 50 | 调试调参帧 (12 个 float PID 参数) |
@@ -126,7 +126,7 @@ main()
| 模式 | 值 | 说明 | | 模式 | 值 | 说明 |
|------|-----|------| |------|-----|------|
| AUTO | 0 | 视觉误差 → KF → vision PID → 角度增量 | | AUTO | 0 | 视觉测量值 → KF → vision PID(measure, ref) → 角度增量 |
| MANUAL | 1 | 上位机下发角速度，固件积分累加角度参考 | | MANUAL | 1 | 上位机下发角速度，固件积分累加角度参考 |
| DEBUG | 2 | 失能电机，接收 PID 参数实时调参 | | DEBUG | 2 | 失能电机，接收 PID 参数实时调参 |
@@ -134,14 +134,14 @@ main()
``` ```
上位机 USART10 → DecodeVision() → vision_data (全局结构体) 上位机 USART10 → DecodeVision() → vision_data (全局结构体)
├── 0xA5 → auto_updated=1, error_x/y ├── 0xA5 → auto_updated=1, measure_x/y, ref_x/y
├── 0xA6 → manual_updated=1, yaw/pitch velocity ├── 0xA6 → manual_updated=1, yaw/pitch velocity
├── 0xA7 → cmd_pending=1, cmd ├── 0xA7 → cmd_pending=1, cmd
└── 0xA8 → debug_updated=1, 12 个 PID 参数 └── 0xA8 → debug_updated=1, 12 个 PID 参数
robot_cmd 每轮: robot_cmd 每轮:
ProcessCommand() → 模式切换 / 激光 ProcessCommand() → 模式切换 / 激光
AUTO: vision_data.error_x/y → KF → vision PID → 角度增量 AUTO: vision_data.measure_x/y → KF → vision PID(measure, ref) → 角度增量
MANUAL: vision_data.yaw/pitch velocity → 角度增量积分 MANUAL: vision_data.yaw/pitch velocity → 角度增量积分
DEBUG: vision_data PID 参数 → 直接写入 vision PID, 发布 motor_pid 话题 DEBUG: vision_data PID 参数 → 直接写入 vision PID, 发布 motor_pid 话题
PubPushMessage(gimbal_cmd) → gimbal 执行 PubPushMessage(gimbal_cmd) → gimbal 执行
@@ -292,7 +292,7 @@ DMMotorControl() @ 1kHz:
3. **伪 Pub-Sub** — 应用间通过 message_center 解耦 3. **伪 Pub-Sub** — 应用间通过 message_center 解耦
4. **Daemon 看门狗** — 每个外设模块独立 DaemonInstance，离线自动回调 4. **Daemon 看门狗** — 每个外设模块独立 DaemonInstance，离线自动回调
5. **BSP 实例化注册** — CAN/串口不注册不初始化 5. **BSP 实例化注册** — CAN/串口不注册不初始化
6. **KF + PID 级联** — 视觉误差经 KF 平滑 → vision PID → 角度增量 → 内环角度 PID 6. **KF + PID 级联** — 视觉测量值经 KF 平滑 → vision PID(measure, ref) → 角度增量 → 内环角度 PID
7. **模式切换重置** — 切换到自动模式时重置 KF 状态 (x=0, P=diag(5,5)) 和 PID 积分项 7. **模式切换重置** — 切换到自动模式时重置 KF 状态 (x=0, P=diag(5,5)) 和 PID 积分项
8. **串口命令控制** — 模式切换、激光控制均通过 0xA7 命令帧触发 8. **串口命令控制** — 模式切换、激光控制均通过 0xA7 命令帧触发
9. **output_reverse** — PID 输出方向统一通过单标志位控制 9. **output_reverse** — PID 输出方向统一通过单标志位控制
@@ -321,7 +321,7 @@ typedef struct {
// 上位机数据 (master_machine.h) // 上位机数据 (master_machine.h)
typedef struct { typedef struct {
float error_x, error_y; // 自动模式: 归一化误差 float measure_x, ref_x, measure_y, ref_y; // 自动模式: 测量值和参考值
uint8_t auto_updated; uint8_t auto_updated;
float yaw_degree_velocity, pitch_degree_velocity; // 手动模式: 角速度 float yaw_degree_velocity, pitch_degree_velocity; // 手动模式: 角速度
uint8_t manual_updated; uint8_t manual_updated;
application/cmd/robot_cmd.c
+51 -42
View File
@@ -19,11 +19,11 @@ static Vision_Mode_e current_mode = VISION_MODE_DEBUG;
static PIDInstance vision_yaw_pid; static PIDInstance vision_yaw_pid;
static PIDInstance vision_pitch_pid; static PIDInstance vision_pitch_pid;
/* 视觉误差卡尔曼滤波器 */ /* 视觉测量值卡尔曼滤波器 */
static KalmanFilter_t yaw_error_kf; static KalmanFilter_t x_measure_kf;
static KalmanFilter_t pitch_error_kf; static KalmanFilter_t y_measure_kf;
static float filtered_yaw_error; static float filtered_x_measure;
static float filtered_pitch_error; static float filtered_y_measure;
/* 内环角度参考累计值 */ /* 内环角度参考累计值 */
static float gimbal_yaw_ref; static float gimbal_yaw_ref;
@@ -69,17 +69,17 @@ static void VisionPIDReset(void)
vision_pitch_pid.Output = 0; vision_pitch_pid.Output = 0;
vision_pitch_pid.Last_Dout = 0; vision_pitch_pid.Last_Dout = 0;
memset(yaw_error_kf.xhat_data, 0, yaw_error_kf.xhatSize * sizeof(float)); memset(x_measure_kf.xhat_data, 0, x_measure_kf.xhatSize * sizeof(float));
memset(yaw_error_kf.P_data, 0, yaw_error_kf.xhatSize * yaw_error_kf.xhatSize * sizeof(float)); memset(x_measure_kf.P_data, 0, x_measure_kf.xhatSize * x_measure_kf.xhatSize * sizeof(float));
memset(pitch_error_kf.xhat_data, 0, pitch_error_kf.xhatSize * sizeof(float)); memset(y_measure_kf.xhat_data, 0, y_measure_kf.xhatSize * sizeof(float));
memset(pitch_error_kf.P_data, 0, pitch_error_kf.xhatSize * pitch_error_kf.xhatSize * sizeof(float)); memset(y_measure_kf.P_data, 0, y_measure_kf.xhatSize * y_measure_kf.xhatSize * sizeof(float));
float P_diag[4] = {5.0f, 0.0f, 0.0f, 5.0f}; float P_diag[4] = {5.0f, 0.0f, 0.0f, 5.0f};
memcpy(yaw_error_kf.P_data, P_diag, sizeof(P_diag)); memcpy(x_measure_kf.P_data, P_diag, sizeof(P_diag));
memcpy(pitch_error_kf.P_data, P_diag, sizeof(P_diag)); memcpy(y_measure_kf.P_data, P_diag, sizeof(P_diag));
filtered_yaw_error = 0; filtered_x_measure = 0;
filtered_pitch_error = 0; filtered_y_measure = 0;
} }
/* ---- 模式切换 ---- */ /* ---- 模式切换 ---- */
@@ -110,8 +110,8 @@ static void SwitchToManualMode(void)
if (current_mode == VISION_MODE_MANUAL) if (current_mode == VISION_MODE_MANUAL)
return; return;
gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle; // gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle;
gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch; // gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch;
vision_data.manual_updated = 0; vision_data.manual_updated = 0;
@@ -175,32 +175,32 @@ static void ProcessCommand(void)
/* ---- KF + PID 初始化 ---- */ /* ---- KF + PID 初始化 ---- */
static void VisionKalmanFilterInit() static void VisionKalmanFilterInit()
{ {
Kalman_Filter_Init(&yaw_error_kf, 2, 0, 1); Kalman_Filter_Init(&x_measure_kf, 2, 0, 1);
static float P_init[4] = {5.0f, 0.0f, 0.0f, 5.0f}; static float P_init[4] = {5.0f, 0.0f, 0.0f, 5.0f};
memcpy(yaw_error_kf.P_data, P_init, sizeof(P_init)); memcpy(x_measure_kf.P_data, P_init, sizeof(P_init));
static float F_init[4] = {1.0f, 0.005f, 0.0f, 1.0f}; static float F_init[4] = {1.0f, 0.005f, 0.0f, 1.0f};
memcpy(yaw_error_kf.F_data, F_init, sizeof(F_init)); memcpy(x_measure_kf.F_data, F_init, sizeof(F_init));
static float Q_init[4] = {0.1f, 0.0f, 0.0f, 1.0f}; static float Q_init[4] = {0.1f, 0.0f, 0.0f, 1.0f};
memcpy(yaw_error_kf.Q_data, Q_init, sizeof(Q_init)); memcpy(x_measure_kf.Q_data, Q_init, sizeof(Q_init));
static uint8_t measurement_map[1] = {1}; static uint8_t measurement_map[1] = {1};
static float measurement_degree[1] = {1.0f}; static float measurement_degree[1] = {1.0f};
static float mat_R_diagonal[1] = {1.0f}; static float mat_R_diagonal[1] = {1.0f};
static float state_min_variance[2] = {0.001f, 0.001f}; static float state_min_variance[2] = {0.001f, 0.001f};
yaw_error_kf.UseAutoAdjustment = 1; x_measure_kf.UseAutoAdjustment = 1;
memcpy(yaw_error_kf.MeasurementMap, measurement_map, sizeof(measurement_map)); memcpy(x_measure_kf.MeasurementMap, measurement_map, sizeof(measurement_map));
memcpy(yaw_error_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree)); memcpy(x_measure_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree));
memcpy(yaw_error_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal)); memcpy(x_measure_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal));
memcpy(yaw_error_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance)); memcpy(x_measure_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance));
Kalman_Filter_Init(&pitch_error_kf, 2, 0, 1); Kalman_Filter_Init(&y_measure_kf, 2, 0, 1);
memcpy(pitch_error_kf.P_data, P_init, sizeof(P_init)); memcpy(y_measure_kf.P_data, P_init, sizeof(P_init));
memcpy(pitch_error_kf.F_data, F_init, sizeof(F_init)); memcpy(y_measure_kf.F_data, F_init, sizeof(F_init));
memcpy(pitch_error_kf.Q_data, Q_init, sizeof(Q_init)); memcpy(y_measure_kf.Q_data, Q_init, sizeof(Q_init));
pitch_error_kf.UseAutoAdjustment = 1; y_measure_kf.UseAutoAdjustment = 1;
memcpy(pitch_error_kf.MeasurementMap, measurement_map, sizeof(measurement_map)); memcpy(y_measure_kf.MeasurementMap, measurement_map, sizeof(measurement_map));
memcpy(pitch_error_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree)); memcpy(y_measure_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree));
memcpy(pitch_error_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal)); memcpy(y_measure_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal));
memcpy(pitch_error_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance)); memcpy(y_measure_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance));
} }
void RobotCMDInit() void RobotCMDInit()
@@ -241,16 +241,25 @@ static void VisionTracking()
{ {
vision_data.auto_updated = 0; vision_data.auto_updated = 0;
yaw_error_kf.MeasuredVector[0] = vision_data.error_x; x_measure_kf.MeasuredVector[0] = vision_data.measure_x;
Kalman_Filter_Update(&yaw_error_kf); Kalman_Filter_Update(&x_measure_kf);
filtered_yaw_error = yaw_error_kf.FilteredValue[0]; filtered_x_measure = x_measure_kf.FilteredValue[0];
pitch_error_kf.MeasuredVector[0] = vision_data.error_y; y_measure_kf.MeasuredVector[0] = vision_data.measure_y;
Kalman_Filter_Update(&pitch_error_kf); Kalman_Filter_Update(&y_measure_kf);
filtered_pitch_error = pitch_error_kf.FilteredValue[0]; filtered_y_measure = y_measure_kf.FilteredValue[0];
float ref_x = vision_data.ref_x;
float yaw_degree_velocity = PIDCalculate(&vision_yaw_pid, filtered_yaw_error, 0); float ref_y = vision_data.ref_y;
float pitch_degree_velocity = PIDCalculate(&vision_pitch_pid, filtered_pitch_error, 0); #if VISION_PID_OUTPUT_YAW_REVERSE == 1
float yaw_degree_velocity = -PIDCalculate(&vision_yaw_pid, filtered_x_measure, ref_x);
#else
float yaw_degree_velocity = PIDCalculate(&vision_yaw_pid, filtered_x_measure, ref_x);
#endif
#if VISION_PID_OUTPUT_PITCH_REVERSE == 1
float pitch_degree_velocity = -PIDCalculate(&vision_pitch_pid, filtered_y_measure, ref_y);
#else
float pitch_degree_velocity = PIDCalculate(&vision_pitch_pid, filtered_y_measure, ref_y);
#endif
gimbal_yaw_ref += 0.001f * ROBOT_CMD_DELAY * yaw_degree_velocity; gimbal_yaw_ref += 0.001f * ROBOT_CMD_DELAY * yaw_degree_velocity;
gimbal_pitch_ref += 0.001f * ROBOT_CMD_DELAY * pitch_degree_velocity; gimbal_pitch_ref += 0.001f * ROBOT_CMD_DELAY * pitch_degree_velocity;
application/cmd/robot_cmd.h
+2
View File
@@ -1,6 +1,8 @@
#ifndef ROBOT_CMD_H #ifndef ROBOT_CMD_H
#define ROBOT_CMD_H #define ROBOT_CMD_H
#define VISION_PID_OUTPUT_YAW_REVERSE 1
#define VISION_PID_OUTPUT_PITCH_REVERSE 0
/** /**
* @brief 机器人核心控制任务初始化,会被RobotInit()调用 * @brief 机器人核心控制任务初始化,会被RobotInit()调用
*/ */
docs/host_comm_guide.md
+12 -10
View File
@@ -15,37 +15,39 @@
| 帧头 | 长度 | 用途 | | 帧头 | 长度 | 用途 |
|---|---|---| |---|---|---|
| 0xA5 | 10 | 自动/视觉追踪数据 | | 0xA5 | 18 | 自动/视觉追踪数据 |
| 0xA6 | 11 | 手动控制数据 | | 0xA6 | 10 | 手动控制数据 |
| 0xA7 | 3 | 命令 | | 0xA7 | 3 | 命令 |
| 0xA8 | 50 | 调试调参数据 | | 0xA8 | 50 | 调试调参数据 |
--- ---
## 1. 自动/视觉追踪数据帧 0xA5 (10 字节) ## 1. 自动/视觉追踪数据帧 0xA5 (18 字节)
**用途**: 视觉识别完成后持续发送跟踪误差。 **用途**: 视觉识别完成后持续发送目标测量值和参考值。固件侧 PID 内部计算误差 (ref - measure)。
``` ```
[SOF:0xA5][error_x:4][error_y:4][checksum:1] [SOF:0xA5][measure_x:4][ref_x:4][measure_y:4][ref_y:4][checksum:1]
``` ```
| 偏移 | 长度 | 类型 | 含义 | | 偏移 | 长度 | 类型 | 含义 |
|---|---|---|---| |---|---|---|---|
| 0 | 1 | uint8 | 帧头 0xA5 | | 0 | 1 | uint8 | 帧头 0xA5 |
| 1 | 4 | float | 归一化 X 轴误差 | | 1 | 4 | float | X 轴测量值 (视觉检测目标位置) |
| 5 | 4 | float | 归一化 Y 轴误差 | | 5 | 4 | float | X 轴参考值 (期望目标位置) |
| 9 | 1 | uint8 | checksum | | 9 | 4 | float | Y 轴测量值 (视觉检测目标位置) |
| 13 | 4 | float | Y 轴参考值 (期望目标位置) |
| 17 | 1 | uint8 | checksum |
**Python 写入示例**: **Python 写入示例**:
```python ```python
import struct import struct
def send_auto_tracking(ser, error_x: float, error_y: float): def send_auto_tracking(ser, measure_x: float, ref_x: float, measure_y: float, ref_y: float):
data = bytearray() data = bytearray()
data.append(0xA5) data.append(0xA5)
data.extend(struct.pack('<ff', error_x, error_y)) data.extend(struct.pack('<ffff', measure_x, ref_x, measure_y, ref_y))
checksum = sum(data) & 0xFF checksum = sum(data) & 0xFF
data.append(checksum) data.append(checksum)
ser.write(data) ser.write(data)
modules/master_machine/master_machine.c
+4 -2
View File
@@ -29,8 +29,10 @@ static void DecodeVision()
{ {
if (calc_checksum(buf, VISION_AUTO_LEN) != buf[VISION_AUTO_LEN - 1]) if (calc_checksum(buf, VISION_AUTO_LEN) != buf[VISION_AUTO_LEN - 1])
return; return;
memcpy(&vision_data.error_x, buf + 1, 4); memcpy(&vision_data.measure_x, buf + 1, 4);
memcpy(&vision_data.error_y, buf + 5, 4); memcpy(&vision_data.ref_x, buf + 5, 4);
memcpy(&vision_data.measure_y, buf + 9, 4);
memcpy(&vision_data.ref_y, buf + 13, 4);
vision_data.auto_updated = 1; vision_data.auto_updated = 1;
} }
else if (sof == VISION_SOF_MANUAL && len == VISION_MANUAL_LEN) else if (sof == VISION_SOF_MANUAL && len == VISION_MANUAL_LEN)
modules/master_machine/master_machine.h
+9 -6
View File
@@ -11,9 +11,10 @@
/* /*
* 上位机串口协议 — 四种帧类型, 通过帧头区分 * 上位机串口协议 — 四种帧类型, 通过帧头区分
* *
* ---- 自动/视觉追踪数据帧 0xA5 (10 字节) ---- * ---- 自动/视觉追踪数据帧 0xA5 (18 字节) ----
* [SOF:0xA5][error_x:4][error_y:4][checksum:1] * [SOF:0xA5][measure_x:4][ref_x:4][measure_y:4][ref_y:4][checksum:1]
* error_x/y: float, 归一化误差, 视觉PID外环反馈值 * measure_x/y: float, 视觉检测目标位置 (测量值)
* ref_x/y: float, 期望目标位置 (参考值)
* *
* ---- 手动控制数据帧 0xA6 (10 字节) ---- * ---- 手动控制数据帧 0xA6 (10 字节) ----
* [SOF:0xA6][yaw_velocity:4][pitch_velocity:4][checksum:1] * [SOF:0xA6][yaw_velocity:4][pitch_velocity:4][checksum:1]
@@ -38,7 +39,7 @@
#define VISION_SOF_CMD 0xA7 #define VISION_SOF_CMD 0xA7
#define VISION_SOF_DEBUG 0xA8 #define VISION_SOF_DEBUG 0xA8
#define VISION_AUTO_LEN 10 #define VISION_AUTO_LEN 18
#define VISION_MANUAL_LEN 10 #define VISION_MANUAL_LEN 10
#define VISION_CMD_LEN 3 #define VISION_CMD_LEN 3
#define VISION_DEBUG_LEN 50 #define VISION_DEBUG_LEN 50
@@ -61,8 +62,10 @@ typedef enum
typedef struct typedef struct
{ {
/* 自动模式数据 */ /* 自动模式数据 */
float error_x; float measure_x;
float error_y; float ref_x;
float measure_y;
float ref_y;
uint8_t auto_updated; // 收到新的自动数据帧标志 uint8_t auto_updated; // 收到新的自动数据帧标志
/* 手动模式数据 */ /* 手动模式数据 */