请问我基于逐飞库写的这个有关于mp u6050传感器的函数，为什么输出的值是固定的？

Question

#include "zf_common_headfile.h"
#include "math.h"

#define DT 0.001
float yijielvbo(float acc_angle, float gyro_angle, float alpha) {
    return alpha * (float)acc_angle + (1 - alpha) * ((float)gyro_angle);
}
float bx, by, bz;
float ax, ay, az;
float pitch, roll, yaw;
float acc_pitch;
float acc_roll;
float acc_yaw;

int main(void)
{
    clock_init(SYSTEM_CLOCK_120M);      // 初始化芯片时钟 工作频率为 120MHz
    debug_init();                       // 务必保留，本函数用于初始化MPU 时钟 调试串口
  
   int i;
    for (i = 1; i <= 200; i++)
    {
        system_delay_ms(5);
     bx=bx + mpu6050_gyro_x/16.4;
     by=by +mpu6050_gyro_y/16.4;
     bz=bz + mpu6050_gyro_z/16.4;
    }
    pit_ms_init(TIM2_PIT, 5); //使用定时器2作为5ms一次的周期中
    while (1)
    {
        printf("%.3f,%.3f,%.3f\n", pitch, roll, yaw);

        // 此处编写需要循环执行的代码
        // 此处编写需要循环执行的代码

    }

}


#define DT 0.005
extern  float acc_pitch;
extern  float acc_roll;
extern  float acc_yaw;
extern  float ax,ay,az;
extern  float bx,by,bz;
extern       float pitch, roll,yaw;
extern float yijielvbo(int16 acc_angle, int16 gyro_angle,float alpha);
float x,y,z;

void TIM2_IRQHandler(void)
    {


        if(TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET)
        { TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
            mpu6050_get_gyro();
            mpu6050_get_acc();
            float gyro_x_dps = mpu6050_gyro_x / 16.4;
            float gyro_y_dps = mpu6050_gyro_y / 16.4;
            float gyro_z_dps = mpu6050_gyro_z / 16.4;
            float acc_x_g = mpu6050_acc_x / 4096;
            float acc_y_g = mpu6050_acc_y / 4096;
            float acc_z_g = mpu6050_acc_z / 4096;
            ax = bx / 200;
            ay = by / 200;
            az = bz / 200;
                    x += (gyro_x_dps - ax) * DT;
                    y += (gyro_y_dps - ay) * DT;
                    z += (gyro_z_dps - az) * DT;

                    acc_pitch = atan2(-acc_y_g, sqrt(acc_x_g * acc_x_g + acc_z_g * acc_z_g)) * 180.0 / PI;
                    acc_roll = atan2(-acc_x_g, sqrt(acc_y_g * acc_y_g + acc_z_g * acc_z_g)) * 180.0 / PI;
                    acc_yaw = atan2(-acc_z_g, sqrt(acc_x_g * acc_x_g + acc_y_g * acc_y_g)) * 180.0 / PI;
           pitch= yijielvbo(acc_pitch,y,0.95);
           roll= yijielvbo(acc_roll, x,0.95);
            yaw= yijielvbo(acc_yaw,z,0.95);
       }
    }

Answer 1

🕒下午好，题主！目前：🟢[在线]
📢参考通义千问、由郭老师整理提供！
📣今日诗号：二十四桥明月夜，玉人何处教吹箫。

从你的代码来看，有几个地方可能导致输出值固定。以下是一些可能的原因和解决方法：

1. 初始化问题

在主函数中，你有一个循环来初始化bx, by, 和 bz。这个循环的目的是为了初始化这些变量。但是，如果这些变量在初始化之前就已经被正确初始化，那么这个循环可能是不必要的。

2. 更新间隔

你定义了一个5ms的定时器中断来更新传感器数据。但是，在主循环中，你直接打印了pitch, roll, 和 yaw，而没有等待定时器中断来更新这些值。因此，这些值可能在主循环中仍然是固定的。

3. 定时器中断处理

你需要确保定时器中断处理程序能够正确地更新pitch, roll, 和 yaw。从你的代码来看，定时器中断处理程序确实会更新这些值，但你需要确保定时器中断被正确触发。

4. 变量更新

在定时器中断处理程序中，你计算了x, y, 和 z，然后用这些值来更新pitch, roll, 和 yaw。你需要确保这些变量在每次中断时都被正确更新。

代码修改建议

主函数

int main(void)
{
    clock_init(SYSTEM_CLOCK_120M);      // 初始化芯片时钟 工作频率为 120MHz
    debug_init();                       // 务必保留，本函数用于初始化MPU 时钟 调试串口

    pit_ms_init(TIM2_PIT, 5); // 使用定时器2作为5ms一次的周期中

    while (1)
    {
        // 等待定时器中断来更新数据
        // 这里不需要做任何事情，因为数据会在定时器中断中更新
    }
}

定时器中断处理程序

void TIM2_IRQHandler(void)
{
    if(TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET)
    {
        TIM_ClearITPendingBit(TIM2, TIM_IT_Update);

        mpu6050_get_gyro();
        mpu6050_get_acc();

        float gyro_x_dps = mpu6050_gyro_x / 16.4;
        float gyro_y_dps = mpu6050_gyro_y / 16.4;
        float gyro_z_dps = mpu6050_gyro_z / 16.4;

        float acc_x_g = mpu6050_acc_x / 4096;
        float acc_y_g = mpu6050_acc_y / 4096;
        float acc_z_g = mpu6050_acc_z / 4096;

        ax = bx / 200;
        ay = by / 200;
        az = bz / 200;

        x += (gyro_x_dps - ax) * DT;
        y += (gyro_y_dps - ay) * DT;
        z += (gyro_z_dps - az) * DT;

        acc_pitch = atan2(-acc_y_g, sqrt(acc_x_g * acc_x_g + acc_z_g * acc_z_g)) * 180.0 / PI;
        acc_roll = atan2(-acc_x_g, sqrt(acc_y_g * acc_y_g + acc_z_g * acc_z_g)) * 180.0 / PI;
        acc_yaw = atan2(-acc_z_g, sqrt(acc_x_g * acc_x_g + acc_y_g * acc_y_g)) * 180.0 / PI;

        pitch = yijielvbo(acc_pitch, y, 0.95);
        roll = yijielvbo(acc_roll, x, 0.95);
        yaw = yijielvbo(acc_yaw, z, 0.95);
    }
}

说明

1. 主函数：移除了不必要的初始化循环，并且去掉了主循环中的打印语句。
2. 定时器中断处理程序：确保传感器数据被正确读取，并且计算出的值被正确更新到pitch, roll, 和 yaw。

通过以上修改，你应该能看到pitch, roll, 和 yaw 的值随着传感器数据的变化而变化。