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自动计算,消除硬编码
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
+6 -3
View File
@@ -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))
{