前言
发现网上树莓派利用dmp读取MPU6050姿态角的方法很少,大部分是直接读取原始数据,然后利用数学方法分析。资料比较多的是stm平台,所以我借鉴了stm的移植方法。
收获最大的是正点原子论坛中关于mpu6050的资料,大家可以在论坛中搜到mpu6050的资料,最终实现在树莓派上读取MPU6050的dmp姿态角数据,要感谢很多前辈,在网上借鉴了很多资料,在此记录一下思路,希望后人不必踩坑
简要分析
因为网上关于移植的例程很多,这里不再赘述(主要是因为懒),这里仅讨论在树莓派上使用dmp硬件解析姿态角中遇到的主要问题
源码贴在github上了:MPU6050-DMP-driver-for-raspi
i2c读写函数主要是依靠wiringPiI2C实现的,所以需要在树莓派上安装有wiringPi。
仅仅是移植驱动很简单,只需要覆写几个主要函数,在inv_mpu.c中说得很清楚:
/* The following functions must be defined for this platform:
* i2c_write(unsigned char slave_addr, unsigned char reg_addr,
* unsigned char length, unsigned char const *data)
* i2c_read(unsigned char slave_addr, unsigned char reg_addr,
* unsigned char length, unsigned char *data)
* delay_ms(unsigned long num_ms)
* get_ms(unsigned long *count)
* reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
* labs(long x)
* fabsf(float x)
* min(int a, int b)*/
下面介绍踩过的坑:
坑1:dmp_load_motion_driver_firmware()报错:
不断追踪 ,最后发现是memcmp(firmware+ii, cur, this_write)校验出错,上网疯狂搜索资料…
查明是i2c时序问题,在对dmp寄存器写入数据的时候,是连续写入16个字节的数据,因为自己覆写的i2c_write函数使用的是wiringPiI2CWriteReg8函数,不支持连续写入数据,所以会报错。
因为使用的是wiringPiI2C写好的通信协议,而wirinigPiI2C中没有支持连续写入或者读取的函数,无奈,深入分析了一下wiringPI2C中的实现方法,都是用i2c_smbus_access来控制通信的,而i2c_smbus_access本身支持进行连续数据读写,只是库中没有写好函数,自己参照i2c驱动函数实现,测试成功。
int i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data)
{
if (length > 32) {
printf("i2_write length too long");
return -2;
}
if (fd < 0)
return -1;
union i2c_smbus_data data_tt;
int i;
if (length > 32)
length = 32;
for (i = 1; i <= length; i++)
data_tt.block[i] = data[i - 1];
data_tt.block[0] = length;
return i2c_smbus_access(fd, I2C_SMBUS_WRITE, reg_addr, I2C_SMBUS_I2C_BLOCK_BROKEN, &data_tt);
}
int i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *values)
{
union i2c_smbus_data data;
int i;
if (length > 32)
length = 32;
data.block[0] = length;
if (i2c_smbus_access(fd, I2C_SMBUS_READ, reg_addr,
length == 32 ? I2C_SMBUS_I2C_BLOCK_BROKEN :
I2C_SMBUS_I2C_BLOCK_DATA, &data))
{
return -1;
}
else {
for (i = 1; i <= data.block[0]; i++)
values[i - 1] = data.block[i];
//return data.block[0];
return 0;
}
}
坑2:mpu_read_fifo_stream报错:
函数mpu_read_fifo_stream报错,在fifo_count < length出错或者 tmp[0] & BIT_FIFO_OVERFLOW出错,这里是fifo溢出问题,具体详细原因网上有分析,大致是读取速度不够快,导致fifo寄存器溢出,又或者读取时间过早,fifo中数据长度还不够,提示fifo_count < length错误,解决方案也简单,疯狂读取,错了继续读,直到读到正确的(这个解决办法很暴力,也很有效)。
while(1){
//在这里疯狂循环读取
while (mpu_dmp_get_data(&pitch, &roll, &yaw) != 0) {}
printf("pitch:%f\troll:%f\tyaw:%f\n",pitch,roll,yaw);
}
坑3:run_self_test()报错
自检报错,大多数情况下把传感器放平就可以自检通过了。
总结
完整的移植后驱动放在了github,主要是inv_mpu.c文件中添加了很多辅助函数。本人才疏学浅,错误难免,仅供参考。