MPU6050 DMP移植到Air724/Air800 Lua语言

MPU6050 DMP移植到Air724/Air800 Lua语言

MPU6050

MPU6050是一个三轴加速度+三轴陀螺仪的六轴传感器，自带DMP数字运动处理器，可以减少MCU复杂的融合演算数据、感测器同步化、姿态感应等负荷。
MPU6050网上的学习资料很多，以正点原子STM32开发板为基础，开始MPU6050的入坑实验。

移植到Air724/Air800 Lua语言

接到一个新的项目，需要计算pitch和roll，且需要低功耗低成本。统筹规划下，就决定Air724/Air800直接外挂MPU6050，通过IIC读写，省去一个MCU的成本和功耗。

MPU6050 DMP计算pitch和roll，说难也难，说简单也简单。

• 第一步，MPU6050 IIC读写
• 第二步，MPU6050 reg读写
• 第三步，MPU6050 mem读写
• 第四步，MPU6050 fifo读写

移植是一件很枯燥的事情，从C语言移植到Lua语言，从STM32移植到Air724/Air800

• 语法修改（function、if、where、for、switch等），很枯燥，很无趣，但又要很小心
• 变量修改，Lua脚本语言不需要定义类型，需要注意的是，局域变量记得定义Local，否则成了全局变量，其他地方也用到就乱了
• 指针修改，DMP库中用了很多指针，这时候就要很小心了，且改动很麻烦。C语言中习惯用指针传参的办法，来获取值。Lua脚本没有指针，但是他函数可以返回多个结果，这就巧妙解决了C语言指针传参取值的问题，通过函数直接返回多个结果值。
• long long修改，Lua没办法做到64位，这个就只能见招拆招，数值不要溢出就行
• 数值脚标，C语言的数组从0开始，Lua的数组从1开始

DMP初始化流程

• IIC初始化
• 读取MPU6050的add==0x68
• mpu_init
  • Initial configuration:
  • Gyro FSR: +/- 2000DPS
  • Accel FSR +/- 2G
  • DLPF: 42Hz
  • FIFO rate: 50Hz
  • Clock source: Gyro PLL
  • FIFO: Disabled.
  • Data ready interrupt: Disabled, active low, unlatched.
• mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL)
• mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL)
• mpu_set_sample_rate 100Hz
• dmp_load_motion_driver_firmware()
• dmp_set_orientation
• dmp_enable_feature
• dmp_set_fifo_rate 100Hz
• run_self_test 自检
• mpu_set_dmp_state 使能dmp

DMP 自检流程

1. 备份MPU当前的参数gyro_fsr、accel_fsr、lpf、sample_rate、sensors、fifo_sensors
2. 根据hw_test=0， 设置寄存器
3. 延时200ms
4. 使能fifo，INV_XYZ_GYRO | INV_XYZ_ACCEL
5. 延时50ms
6. 读取全部fifo，再平均。存入accel、gyro
7. 根据hw_test=1，执行第2步到第6步。存入accel_st、gyro_st
8. accel和accel_st的差值，经过计算要比0.14小，否则accel自检失败
9. gyro和gyro_st的差值，经过计算要比0.14小，否则gyro自检失败
10. 将备份参数重新设置到MPU寄存器中，并使能dmp
11. 将gyro*gyro_sens，写入gyro bais中
12. 将accel*accel_sens，写入accel bais中

关于自检再多说两句

1. 自检过程中，需要保持静止，不可晃动，否则自检容易失败
2. 不需要平放也能通过自检，但是计算出来的pitch，roll是错的
3. 自检的时候，会将pitch、roll、yaw归零

DMP运行流程

1. mpu_read_fifo_stream （注意溢出）
2. 提取quat
3. 通过quat计算quat_mag_sq，判断是否超过阈值
4. quat四元数再转成pitch、roll、yaw

		local q0 = quat[1] / q30	--//q30格式转换为浮点数
		local q1 = quat[2] / q30
		local q2 = quat[3] / q30
		local q3 = quat[4] / q30 
		pitch = math.asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3	 
		roll  = math.atan(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3 
		yaw   = math.atan(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3	 

注意溢出

1. fifo使能后，mpu6050就会不停地计算并存入fifo，如果短时间不去读取fifo的话，就容易出现fifo溢出的情况。
2. fifo溢出的时候，数据是不正确的，需要mpu_reset_fifo，然后再去读取fifo

现存的问题

花费了将近一个星期的时间，遇到不少问题，也解决不少问题，移植工作接近尾声，MPU6050 初始化、自检、DMP读数，四元数计算都没问题。但是最后还现存一个问题。
现存问题：pitch 范围只有±45°，roll范围只有±90°，yaw范围只有±90°。pitch转到90°时，读数是45°（错的），再转开始变小44°、43°、yaw转到90°时，读数是90°，再转开始变小89°、88°。
排查问题：从atan计算到quat计算，都没问题，问题出在mpu_read_fifo_stream读出来的原始数据上，然后我也怀疑是accel_sens、accel_fsr、gyro_fsr、accel_half等问题，一一对比过，都没问题。
十分纳闷，心里堵着慌，这是为啥子呢，不过因为手头工作还很多，这个问题就先用线性映射的方法绕过去了，后面有时候再来好好排查下。如果各位大神有办法和思路，欢迎指导，不吝赐教。