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