近来使用了MPU6050芯片，芯片本身带有自检功能，网上搜了一下发现并没有现成的程序，便自己写了一个，分享给大家！请看源代码：
[mw_shl_code=c,true]u8 MPU_SelfTest(void)
{
s16 ax,ay,az,gx,gy,gz,str_ax,str_ay,str_az,str_gx,str_gy,str_gz,t;
u8 buff[4]={0},test_ax,test_ay,test_az,test_gx,test_gy,test_gz;
float a;
MPU_WriteData(MPU_GYRO_CFG_REG,0xe0); //设置陀螺仪三轴自检，并设置满了量程范围为±250dps
MPU_WriteData(MPU_ACCEL_CFG_REG,0xf0); //设置加速度计三轴自检，并设置满了量程范围为±8g
MPU_GetOrigData(&str_ax,&str_ay,&str_az,&str_gx,&str_gy,&str_gz,&t); //读取数据
MPU_WriteData(MPU_GYRO_CFG_REG,0x00); //设置陀螺仪三轴不自检，并设置满了量程范围为±250dps
MPU_WriteData(MPU_ACCEL_CFG_REG,0x10); //设置加速度计三轴不自检，并设置满了量程范围为±8g
MPU_GetOrigData(&ax,&ay,&az,&gx,&gy,&gz,&t); //读取数据
//计算STR
str_ax -= ax;
str_ay -= ay;
str_az -= az;
str_gx -= gx;
str_gy -= gy;
str_gz -= gz;
//读取自检寄存器
IIC_ReadData(buff,4,MPU6050_ADDR,MPU_SELF_TESTX_REG);
test_gx = buff[0] & 0x1f;
test_gy = buff[1] & 0x1f;
test_gz = buff[2] & 0x1f;
test_ax = ((buff[0] & 0xe0)>>3)+((buff[3]&0x30)>>4);
test_ay = ((buff[1] & 0xe0)>>3)+((buff[3]&0x0c)>>2);
test_az = ((buff[2] & 0xe0)>>3)+(buff[3]&0x03);
//计算FT
gx = test_gx==0 ? 0:25*131*pow(1.046,test_gx-1);
gy = test_gy==0 ? 0:-25*131*pow(1.046,test_gy-1);
gz = test_gz==0 ? 0:25*131*pow(1.046,test_gz-1);
ax = test_ax==0 ? 0:4096*0.34*pow(0.92/0.34,(test_ax-1)/30.0);
ay = test_ay==0 ? 0:4096*0.34*pow(0.92/0.34,(test_ay-1)/30.0);
az = test_az==0 ? 0:4096*0.34*pow(0.92/0.34,(test_az-1)/30.0);
//计算自检响应
mpu6050.Acc_selftest.x = ((float)(str_ax-ax)/ax);
mpu6050.Acc_selftest.y = ((float)(str_ay-ay)/ay);
mpu6050.Acc_selftest.z = ((float)(str_az-az)/az);
mpu6050.Gyro_selftest.x = ((float)(str_gx-gx)/gx);
mpu6050.Gyro_selftest.y = ((float)(str_gy-gy)/gy);
mpu6050.Gyro_selftest.z = ((float)(str_gz-gz)/gz);
MPU_WriteData(MPU_GYRO_CFG_REG,3<<3); //恢复初始设置
MPU_WriteData(MPU_ACCEL_CFG_REG,0<<3);
if(fabs(mpu6050.Acc_selftest.x)<14 && fabs(mpu6050.Acc_selftest.y)<14 && fabs(mpu6050.Acc_selftest.z)<14 && fabs(mpu6050.Gyro_selftest.x)<14 && fabs(mpu6050.Gyro_selftest.y)<14 && fabs(mpu6050.Gyro_selftest.z)<14)
return 0;
else
return 1;
}[/mw_shl_code]
MPU6050数据手册和寄存器手册上也说的非常明白了，值得注意的是：自检时陀螺仪量程必须是±250dps，加速度计量程必须是±8g，mpu6050.Acc_selftest.x、y、z，mpu6050.Gyro_selftest.x、y、z的绝对值小于14则自检通过。
程序中有几个函数没有给出源代码，现解释如下（其实看名称就能理解）：
MPU_WriteData：使用IIC写数据给MPU6050，第一个参数是寄存器地址，第二个参数是要写入的数据；
MPU_GetOrigData：获取MPU6050三轴加速度和角速度值
IIC_ReadData：连续读取数据，第一个参数是读入的内存地址，第二个参数是读取的个数，第三个参数是MPU6050器件地址，第四个参数是寄存器地址。
自身水平有限，如有错误，请多多指教。 |
楼主
标题置顶
标题高亮
