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:
@@ -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))
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user