diff --git a/README.md b/README.md index 37b7da0..c0b078d 100644 --- a/README.md +++ b/README.md @@ -67,7 +67,7 @@ main() ┌──────────────────────────────────────────────────────┐ │ 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 │ │ MANUAL: 上位机 0xA6 帧 → yaw/pitch 角速度 → 角度增量积分 │ │ 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) | | 0xA7 | 3 | 命令帧 (模式切换, 激光) | | 0xA8 | 50 | 调试调参帧 (12 个 float PID 参数) | @@ -126,7 +126,7 @@ main() | 模式 | 值 | 说明 | |------|-----|------| -| AUTO | 0 | 视觉误差 → KF → vision PID → 角度增量 | +| AUTO | 0 | 视觉测量值 → KF → vision PID(measure, ref) → 角度增量 | | MANUAL | 1 | 上位机下发角速度，固件积分累加角度参考 | | DEBUG | 2 | 失能电机，接收 PID 参数实时调参 | @@ -134,14 +134,14 @@ main() ``` 上位机 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 ├── 0xA7 → cmd_pending=1, cmd └── 0xA8 → debug_updated=1, 12 个 PID 参数 robot_cmd 每轮: 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 → 角度增量积分 DEBUG: vision_data PID 参数 → 直接写入 vision PID, 发布 motor_pid 话题 PubPushMessage(gimbal_cmd) → gimbal 执行 @@ -292,7 +292,7 @@ DMMotorControl() @ 1kHz: 3. **伪 Pub-Sub** — 应用间通过 message_center 解耦 4. **Daemon 看门狗** — 每个外设模块独立 DaemonInstance，离线自动回调 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 积分项 8. **串口命令控制** — 模式切换、激光控制均通过 0xA7 命令帧触发 9. **output_reverse** — PID 输出方向统一通过单标志位控制 @@ -321,7 +321,7 @@ typedef struct { // 上位机数据 (master_machine.h) typedef struct { - float error_x, error_y; // 自动模式: 归一化误差 + float measure_x, ref_x, measure_y, ref_y; // 自动模式: 测量值和参考值 uint8_t auto_updated; float yaw_degree_velocity, pitch_degree_velocity; // 手动模式: 角速度 uint8_t manual_updated; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index fe41f27..21a1677 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -19,11 +19,11 @@ static Vision_Mode_e current_mode = VISION_MODE_DEBUG; static PIDInstance vision_yaw_pid; static PIDInstance vision_pitch_pid; -/* 视觉误差卡尔曼滤波器 */ -static KalmanFilter_t yaw_error_kf; -static KalmanFilter_t pitch_error_kf; -static float filtered_yaw_error; -static float filtered_pitch_error; +/* 视觉测量值卡尔曼滤波器 */ +static KalmanFilter_t x_measure_kf; +static KalmanFilter_t y_measure_kf; +static float filtered_x_measure; +static float filtered_y_measure; /* 内环角度参考累计值 */ static float gimbal_yaw_ref; @@ -69,17 +69,17 @@ static void VisionPIDReset(void) vision_pitch_pid.Output = 0; vision_pitch_pid.Last_Dout = 0; - memset(yaw_error_kf.xhat_data, 0, yaw_error_kf.xhatSize * sizeof(float)); - memset(yaw_error_kf.P_data, 0, yaw_error_kf.xhatSize * yaw_error_kf.xhatSize * sizeof(float)); - memset(pitch_error_kf.xhat_data, 0, pitch_error_kf.xhatSize * sizeof(float)); - memset(pitch_error_kf.P_data, 0, pitch_error_kf.xhatSize * pitch_error_kf.xhatSize * sizeof(float)); + memset(x_measure_kf.xhat_data, 0, x_measure_kf.xhatSize * sizeof(float)); + memset(x_measure_kf.P_data, 0, x_measure_kf.xhatSize * x_measure_kf.xhatSize * sizeof(float)); + memset(y_measure_kf.xhat_data, 0, y_measure_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}; - memcpy(yaw_error_kf.P_data, P_diag, sizeof(P_diag)); - memcpy(pitch_error_kf.P_data, P_diag, sizeof(P_diag)); + memcpy(x_measure_kf.P_data, P_diag, sizeof(P_diag)); + memcpy(y_measure_kf.P_data, P_diag, sizeof(P_diag)); - filtered_yaw_error = 0; - filtered_pitch_error = 0; + filtered_x_measure = 0; + filtered_y_measure = 0; } /* ---- 模式切换 ---- */ @@ -110,8 +110,8 @@ static void SwitchToManualMode(void) if (current_mode == VISION_MODE_MANUAL) return; - gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle; - gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch; + // gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle; + // gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch; vision_data.manual_updated = 0; @@ -175,32 +175,32 @@ static void ProcessCommand(void) /* ---- KF + PID 初始化 ---- */ 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}; - 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}; - 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}; - 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 float measurement_degree[1] = {1.0f}; static float mat_R_diagonal[1] = {1.0f}; static float state_min_variance[2] = {0.001f, 0.001f}; - yaw_error_kf.UseAutoAdjustment = 1; - memcpy(yaw_error_kf.MeasurementMap, measurement_map, sizeof(measurement_map)); - memcpy(yaw_error_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree)); - memcpy(yaw_error_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal)); - memcpy(yaw_error_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance)); + x_measure_kf.UseAutoAdjustment = 1; + memcpy(x_measure_kf.MeasurementMap, measurement_map, sizeof(measurement_map)); + memcpy(x_measure_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree)); + memcpy(x_measure_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal)); + memcpy(x_measure_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance)); - Kalman_Filter_Init(&pitch_error_kf, 2, 0, 1); - memcpy(pitch_error_kf.P_data, P_init, sizeof(P_init)); - memcpy(pitch_error_kf.F_data, F_init, sizeof(F_init)); - memcpy(pitch_error_kf.Q_data, Q_init, sizeof(Q_init)); - pitch_error_kf.UseAutoAdjustment = 1; - memcpy(pitch_error_kf.MeasurementMap, measurement_map, sizeof(measurement_map)); - memcpy(pitch_error_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree)); - memcpy(pitch_error_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal)); - memcpy(pitch_error_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance)); + Kalman_Filter_Init(&y_measure_kf, 2, 0, 1); + memcpy(y_measure_kf.P_data, P_init, sizeof(P_init)); + memcpy(y_measure_kf.F_data, F_init, sizeof(F_init)); + memcpy(y_measure_kf.Q_data, Q_init, sizeof(Q_init)); + y_measure_kf.UseAutoAdjustment = 1; + memcpy(y_measure_kf.MeasurementMap, measurement_map, sizeof(measurement_map)); + memcpy(y_measure_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree)); + memcpy(y_measure_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal)); + memcpy(y_measure_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance)); } void RobotCMDInit() @@ -241,16 +241,25 @@ static void VisionTracking() { vision_data.auto_updated = 0; - yaw_error_kf.MeasuredVector[0] = vision_data.error_x; - Kalman_Filter_Update(&yaw_error_kf); - filtered_yaw_error = yaw_error_kf.FilteredValue[0]; + x_measure_kf.MeasuredVector[0] = vision_data.measure_x; + Kalman_Filter_Update(&x_measure_kf); + filtered_x_measure = x_measure_kf.FilteredValue[0]; - pitch_error_kf.MeasuredVector[0] = vision_data.error_y; - Kalman_Filter_Update(&pitch_error_kf); - filtered_pitch_error = pitch_error_kf.FilteredValue[0]; - - float yaw_degree_velocity = PIDCalculate(&vision_yaw_pid, filtered_yaw_error, 0); - float pitch_degree_velocity = PIDCalculate(&vision_pitch_pid, filtered_pitch_error, 0); + y_measure_kf.MeasuredVector[0] = vision_data.measure_y; + Kalman_Filter_Update(&y_measure_kf); + filtered_y_measure = y_measure_kf.FilteredValue[0]; + float ref_x = vision_data.ref_x; + float ref_y = vision_data.ref_y; + #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_pitch_ref += 0.001f * ROBOT_CMD_DELAY * pitch_degree_velocity; diff --git a/application/cmd/robot_cmd.h b/application/cmd/robot_cmd.h index 0f2584e..f7d4528 100644 --- a/application/cmd/robot_cmd.h +++ b/application/cmd/robot_cmd.h @@ -1,6 +1,8 @@ #ifndef ROBOT_CMD_H #define ROBOT_CMD_H +#define VISION_PID_OUTPUT_YAW_REVERSE 1 +#define VISION_PID_OUTPUT_PITCH_REVERSE 0 /** * @brief 机器人核心控制任务初始化,会被RobotInit()调用 */ diff --git a/docs/host_comm_guide.md b/docs/host_comm_guide.md index a8b62dc..e854dfa 100644 --- a/docs/host_comm_guide.md +++ b/docs/host_comm_guide.md @@ -15,37 +15,39 @@ | 帧头 | 长度 | 用途 | |---|---|---| -| 0xA5 | 10 | 自动/视觉追踪数据 | -| 0xA6 | 11 | 手动控制数据 | +| 0xA5 | 18 | 自动/视觉追踪数据 | +| 0xA6 | 10 | 手动控制数据 | | 0xA7 | 3 | 命令 | | 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 | -| 1 | 4 | float | 归一化 X 轴误差 | -| 5 | 4 | float | 归一化 Y 轴误差 | -| 9 | 1 | uint8 | checksum | +| 1 | 4 | float | X 轴测量值 (视觉检测目标位置) | +| 5 | 4 | float | X 轴参考值 (期望目标位置) | +| 9 | 4 | float | Y 轴测量值 (视觉检测目标位置) | +| 13 | 4 | float | Y 轴参考值 (期望目标位置) | +| 17 | 1 | uint8 | checksum | **Python 写入示例**: ```python 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.append(0xA5) - data.extend(struct.pack('