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
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_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;