MPU6050初始化及数据过滤，滤波效果

最新推荐文章于 2025-06-25 19:37:53 发布

原创

最新推荐文章于 2025-06-25 19:37:53 发布 · 3.5k 阅读

12 ·

CC 4.0 BY-SA版权

文章标签：

#树莓派 #MPU6050 #滤波 #飞控 #姿态

接上一篇，这里说下树莓派飞控的硬件初始化以及过滤数据中的噪音。飞机在高速转动下，陀螺仪和加速度计由于噪音的存在，传递给解算姿态的数据也会有问题，导致解算姿态结果不理想。

实验环境

树莓派，mpu6050，qmc5883l，接口都是使用IIC（建议都改过树莓派iic的传输速率，默认只有100k/s，速度不够快。

代码初始化

6050的初始化不算难，主要设置陀螺仪，加速度计量程，采集频率等基本也够了，github上很多也是这样，设置到位，数据也就对了。

 bool mpu6050Init() {
   
   
 
     if (isInit)
       return true;
     _DEBUG(DEBUG_NORMAL, "Resetting MPU6050 ...\n");
     reset();
     usleep(120000); // wait after reset
   // Clear sleep mode bit (6), enable all sensors
     i2cdev_writeByte(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x00);
     usleep(1000);
   // Delay 100 ms for PLL to get established on x-axis gyro;
   //should check for PLL ready
     _DEBUG(DEBUG_NORMAL, "Disabling sleep mode...\n");
     setSleepEnabled(false);
     usleep(1000);
 
     setClockSource(MPU6050_CLOCK_PLL_XGYRO);
     usleep(1000);
     unsigned char c;
     i2cdev_readByte(MPU6050_ADDRESS, MPU6050_CLOCK_PLL_XGYRO, &c);
     _DEBUG(DEBUG_NORMAL, "Setting clock source to x Gyro...,%d\n", c);
 
     setRate(0x00); // 1khz / (1 + 0) = 1k Hz
     usleep(1000);
     i2cdev_readByte(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, &c);
     _DEBUG(DEBUG_NORMAL, "Setting sample rate to 1k Hz...%d\n", c);