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自动计算，消除硬编码

project/code/Command.c

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

project/code/Task.c

@@ -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,6 +50,7 @@ void Task_10ms(uint32 state, void *ptr)
 {
     // 10ms 执行一次的任务代码(100Hz)
     motor_encoder_update();
+    imu_update();
     if(motor_control_task_enable)
         motor_control_task();
 
@@ -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);
+    motor_control_init(TASK_10MS_FREQ_HZ);
-    angle_control_init(0.02f);
+    angle_control_init(TASK_20MS_FREQ_HZ);
-    position_control_init(0.02f);
+    position_control_init(TASK_20MS_FREQ_HZ);
 
-    pit_ms_init(PIT_TIM_A0,10,Task_10ms,NULL);
+    pit_ms_init(PIT_TIM_A0, (uint32_t)(mc_dt * 1000.0f), Task_10ms, NULL);
-    interrupt_set_priority(TIMA0_INT_IRQn,1);
+    interrupt_set_priority(TIMA0_INT_IRQn, 1);
-    pit_ms_init(PIT_TIM_A1,20,Task_20ms,NULL);
+    pit_ms_init(PIT_TIM_A1, (uint32_t)(ac_dt * 1000.0f), Task_20ms, NULL);
-    interrupt_set_priority(TIMA1_INT_IRQn,1);
+    interrupt_set_priority(TIMA1_INT_IRQn, 1);
-    pit_ms_init(PIT_TIM_G6,50,Task_50ms,NULL);
+    pit_ms_init(PIT_TIM_G6, (uint32_t)(1000.0f / TASK_50MS_FREQ_HZ), Task_50ms, NULL);
-    interrupt_set_priority(TIMG6_INT_IRQn,7);
+    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 uint8_t position_turn_right_90_runing = 0;
-volatile static float position_turn_target_yaw = 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_VELOCITY (-57)
-#define POSITION_TURN_ANGLE_INCREASE 85
+#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);

project/code/Task.h

@@ -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;
 //配置项

project/user/src/main.c

@@ -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};
+        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\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))
         {