MPU6050 DMP移植到Air724/Air800 Lua语言

MPU6050 DMP移植到Air724/Air800 Lua语言

MPU6050

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

MPU6050 DMP移植到Air724/Air800 Lua语言_第2张图片

移植到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等问题,一一对比过,都没问题。
十分纳闷,心里堵着慌,这是为啥子呢,不过因为手头工作还很多,这个问题就先用线性映射的方法绕过去了,后面有时候再来好好排查下。如果各位大神有办法和思路,欢迎指导,不吝赐教。

MPU6050 DMP移植到Air724/Air800 Lua语言_第3张图片

你可能感兴趣的:(杂七杂八)