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

根因：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自动计算，消除硬编码
2026-06-07 20:10:05 +08:00
parent 49b0dc637c
commit 7260fe9fba
4 changed files with 70 additions and 32 deletions
@@ -158,6 +158,9 @@ void SwitchToAutoMode()
    //设定基础直行速度
    motor_speed_base=AUTO_MODE_BASE_SPEED;

+    //清理子任务标志
+    position_turn_right_90_runing=0;
+
    /* 开启控制任务 */
    motor_control_task_enable=1;
    angle_control_task_enable=1;
@@ -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);
@@ -5,17 +5,29 @@

 #include "PID_Controller.h"

+//==================================================================================================
+// 任务频率配置（统一入口，修改这里即可全局生效）
+//==================================================================================================
+#define TASK_10MS_FREQ_HZ  100   // 10ms 任务：电机编码器、电机控制、IMU 姿态更新
+#define TASK_20MS_FREQ_HZ  50    // 20ms 任务：灰度传感器、位置环、角度环
+#define TASK_50MS_FREQ_HZ  20    // 50ms 任务：按键扫描、蜂鸣器、指令处理
+
 void task_init(void);

-//建议不直接修改此变量，而是通过set_yaw_ref(float set_value)设置，以防预期之外的值
-extern volatile float yaw_ref;
 void set_yaw_ref(float set_value);

+//子系统初始化（传入频率 Hz，内部存储频率和周期）
+void motor_control_init(float freq_hz);
+void angle_control_init(float freq_hz);
+void position_control_init(float freq_hz);
+
 //供外部控制使用
 extern volatile uint8_t motor_control_task_enable;
 extern volatile uint8_t angle_control_task_enable;
 extern volatile uint8_t position_control_task_enable;

+extern volatile uint8_t position_turn_right_90_runing;
+
 extern volatile PIDInstance pid_speed_right;
 extern volatile PIDInstance pid_speed_left;
 extern volatile PIDInstance pid_yaw;
@@ -27,7 +39,7 @@ extern volatile int16_t motor_speed_left_ref;
 /*角度环接管速度之后的基础直行速度*/
 extern volatile int16_t motor_speed_base;
 /*角度环参考值*/
-extern volatile float yaw_ref;
+extern volatile float yaw_ref;//建议不直接修改此变量，而是通过set_yaw_ref(float set_value)设置，以防预期之外的值
 /*角度速率控制，开启位置环后由位置环接管*/
 extern volatile float angle_velocity;
 //配置项
@@ -72,7 +72,7 @@ int main (void)
    beep_init(50);
    motor_init();
    key_init(50);//按键初始化，参数为扫描周期50ms
-    imu_init(200);// IMU 初始化，参数为采样频率 200Hz 需要与定时器中断频率一致
+    imu_init(TASK_10MS_FREQ_HZ); // IMU 初始化，采样频率需与所在任务频率一致（当前在 10ms=100Hz 任务中）
    imu_calibrate_gyro(); // IMU 陀螺仪零偏校准 需要在静止状态下调用
    grayscale_sensor_init();//灰度传感器初始化
    motor_encoder_init();//电机编码器初始化
@@ -82,12 +82,15 @@ int main (void)

    beep_on_time(500);//初始化完成，蜂鸣器响 500ms
    // 此处编写用户代码 例如外设初始化代码等
+    extern volatile float position_turn_target_yaw;
    while(1)
    {
        // 此处编写需要循环执行的代码
        system_delay_ms(100);
-        char str[20] = {0};
-        sprintf(str,"%d,%d,%d\n", (int)(imu_angle.yaw*100),(int)(yaw_ref*100),(int)(grayscale_sensor_position*100));
+        char str[32] = {0};
+        //sprintf(str,"%d,%d,%d\n", (int)(imu_angle.yaw*100),(int)(yaw_ref*100),(int)(grayscale_sensor_position*100));
+        //sprintf(str,"%d,%d,%d,%d,%d,%d,%d,%d\n",(int)(adc_normalized[0]*100),(int)(adc_normalized[1]*100),(int)(adc_normalized[2]*100),(int)(adc_normalized[3]*100),(int)(adc_normalized[4]*100),(int)(adc_normalized[5]*100),(int)(adc_normalized[6]*100),(int)(adc_normalized[7]*100));
+        sprintf(str,"%d,%d,%d\n",(int)(position_turn_target_yaw*100),(int)(imu_angle.yaw*100),(int)(yaw_ref*100));
        debug_send_buffer((const uint8 *)str,strlen(str));
        if(KEY_SHORT_PRESS == key_get_state(KEY_1))
        {