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

fix: 修复IMU采样频率不匹配导致yaw角积分偏小4倍的问题

Browse Source 
根因：imu_init(200) 声明200Hz，但imu_update实际在50Hz任务中调用，
Mahony积分dt差4倍，导致转90度只测出22度。
修复：imu_update移至10ms任务(100Hz)，传感器采集与控制频率对齐。

同时将任务周期全部参数化：
- Task.h 统一定义 TASK_10MS/20MS/50MS_FREQ_HZ
- 各子系统init改为传入频率(Hz)，内部算周期(dt)
- PIT定时器周期由存储的dt自动计算，消除硬编码
This commit is contained in:
李傲琦
2026-06-07 20:10:05 +08:00
parent 49b0dc637c
commit 7260fe9fba
4 changed files with 70 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
project/code/Task.c
+46 -26
View File
@@ -14,6 +14,16 @@ volatile uint8_t motor_control_task_enable = 0;
volatile uint8_t angle_control_task_enable = 0;
volatile uint8_t position_control_task_enable = 0;
//==================================================================================================
// 子系统频率/周期存储（由 *_init 函数填入，其他函数按需读取）
//==================================================================================================
static float mc_freq = 0.0f; // 电机控制频率 (Hz)
static float mc_dt = 0.0f; // 电机控制周期 (s)
static float ac_freq = 0.0f; // 角度控制频率 (Hz)
static float ac_dt = 0.0f; // 角度控制周期 (s)
static float pc_freq = 0.0f; // 位置控制频率 (Hz)
static float pc_dt = 0.0f; // 位置控制周期 (s)
/*速度环的输入，开启角度环后由角度环接管(范围-50到+50)*/
volatile int16_t motor_speed_right_ref = 0;
volatile int16_t motor_speed_left_ref = 0;
@@ -30,9 +40,6 @@ volatile PIDInstance pid_yaw = {0};
volatile PIDInstance pid_position = {0};
void motor_control_init(float T);
void angle_control_init(float T);
void position_control_init(float T);
void angle_control_task(void);
void position_control_task(void);
void motor_control_task(void);
@@ -43,9 +50,10 @@ void Task_10ms(uint32 state, void *ptr)
{
// 10ms 执行一次的任务代码(100Hz)
motor_encoder_update();
imu_update();
if(motor_control_task_enable)
motor_control_task();
}
void Task_20ms(uint32 state, void *ptr)
{
@@ -54,7 +62,6 @@ void Task_20ms(uint32 state, void *ptr)
if(position_control_task_enable)
position_control_task();
imu_update();
if(angle_control_task_enable)
angle_control_task();
}
@@ -67,25 +74,28 @@ void Task_50ms(uint32 state, void *ptr)
}
void task_init(void)
{
motor_control_init(0.01f);
angle_control_init(0.02f);
position_control_init(0.02f);
motor_control_init(TASK_10MS_FREQ_HZ);
angle_control_init(TASK_20MS_FREQ_HZ);
position_control_init(TASK_20MS_FREQ_HZ);
pit_ms_init(PIT_TIM_A0,10,Task_10ms,NULL);
interrupt_set_priority(TIMA0_INT_IRQn,1);
pit_ms_init(PIT_TIM_A1,20,Task_20ms,NULL);
interrupt_set_priority(TIMA1_INT_IRQn,1);
pit_ms_init(PIT_TIM_G6,50,Task_50ms,NULL);
interrupt_set_priority(TIMG6_INT_IRQn,7);
pit_ms_init(PIT_TIM_A0, (uint32_t)(mc_dt * 1000.0f), Task_10ms, NULL);
interrupt_set_priority(TIMA0_INT_IRQn, 1);
pit_ms_init(PIT_TIM_A1, (uint32_t)(ac_dt * 1000.0f), Task_20ms, NULL);
interrupt_set_priority(TIMA1_INT_IRQn, 1);
pit_ms_init(PIT_TIM_G6, (uint32_t)(1000.0f / TASK_50MS_FREQ_HZ), Task_50ms, NULL);
interrupt_set_priority(TIMG6_INT_IRQn, 7);
}
void motor_control_init(float T)
void motor_control_init(float freq_hz)
{
mc_freq = freq_hz;
mc_dt = 1.0f / freq_hz;
PID_Init_Config_s config_right = {
.Kp = 300.0f,
.Ki = 150.0f,
.Kd = 0.0f,
.T = T,
.T = mc_dt,
.MaxOut = MOTOR_MAX
};
PIDInit((PIDInstance *)&pid_speed_right, &config_right);
@@ -93,7 +103,7 @@ void motor_control_init(float T)
.Kp = 300.0f,
.Ki = 150.0f,
.Kd = 0.0f,
.T = T,
.T = mc_dt,
.MaxOut = MOTOR_MAX
};
PIDInit((PIDInstance *)&pid_speed_left, &config_left);
@@ -106,13 +116,16 @@ void motor_control_task(void)
motor_set_pwm_right(pwm_right);
motor_set_pwm_left(pwm_left);
}
void angle_control_init(float T)
void angle_control_init(float freq_hz)
{
ac_freq = freq_hz;
ac_dt = 1.0f / freq_hz;
PID_Init_Config_s config_yaw = {
.Kp = 1.0f,
.Ki = 0.0f,
.Kd = 0.0f,
.T = T,
.T = ac_dt,
.MaxOut = 25,
.Improve = PID_Yaw_Error_Corretion | PID_Derivative_On_Measurement
};
@@ -127,24 +140,28 @@ void angle_control_task(void)
motor_speed_left_ref = motor_speed_base - speed_dif;
motor_speed_right_ref = motor_speed_base + speed_dif;
}
void position_control_init(float T)
void position_control_init(float freq_hz)
{
pc_freq = freq_hz;
pc_dt = 1.0f / freq_hz;
PID_Init_Config_s config_position = {
.Kp = 0.5f,
.Ki = 0.0f,
.Kd = 0.0f,
.T = T,
.T = pc_dt,
.MaxOut = 90,
};
PIDInit((PIDInstance *)&pid_position, &config_position);
}
/*位置环子任务 BEGIN*/
//右转90度，按需开启，开启后自动检测条件结束
volatile static uint8_t position_turn_right_90_runing = 0;
volatile static float position_turn_target_yaw = 0;
volatile uint8_t position_turn_right_90_runing = 0;
volatile float position_turn_target_yaw = 0;
#define check_turn_right_90() (GS_IS_ON_BLACK(GS_R4) && GS_IS_ON_BLACK(GS_R3) && GS_IS_ON_BLACK(GS_R2))
#define POSITION_TURN_ANGLE_VELOCITY 37
#define POSITION_TURN_ANGLE_INCREASE 85
#define POSITION_TURN_ANGLE_VELOCITY (-57)
#define POSITION_TURN_ANGLE_INCREASE (-85)
#define POSITION_TURN_ANGLE_INCREASE_CHECK_THRESHOLD (5.0f)
/*位置环子任务 END*/
void position_control_task(void)
{
@@ -156,6 +173,7 @@ void position_control_task(void)
//记录要到达的角度
position_turn_target_yaw = imu_angle.yaw + POSITION_TURN_ANGLE_INCREASE;
yaw_correct((float *)&position_turn_target_yaw);
beep_on_time(100);
}
//如果右转90度标志位启用，接管角度速率，固定速度右转
if(position_turn_right_90_runing)
@@ -163,8 +181,9 @@ void position_control_task(void)
//检测角度，如果完成90度，则取消右转90度标志位
float sub = position_turn_target_yaw - imu_angle.yaw;
yaw_correct(&sub);
if(sub<=0)
if(-sub<POSITION_TURN_ANGLE_INCREASE_CHECK_THRESHOLD)
{
beep_on_time(100);
position_turn_right_90_runing=0;
//取消后立即进行一次计算
angle_velocity = PIDCalculate((PIDInstance *)&pid_position,grayscale_sensor_position,0);
@@ -181,6 +200,7 @@ void position_control_task(void)
command_send_angle_velocity((float*)&angle_velocity);
}
void set_yaw_ref(float set_value)
{
yaw_correct(&set_value);