| MPU6050内部DMP固件移植解析，STM32获取欧拉角串口显示 | 您所在的位置：网站首页 › DMP-10是什么 › MPU6050内部DMP固件移植解析，STM32获取欧拉角串口显示 |
MPU6050内部DMP固件移植解析，STM32获取欧拉角串口显示
|
MPU6050模块是块好东西大伙都知道，围绕这个几块钱的东西就可以做很多很好玩的东西，什么四翼飞行器、平衡车等。当然要完全使用这块模块不是那么容易。 官方原版驱动下载地址： http://drivers.softpedia.com/get/Other-DRIVERS-TOOLS/Others/InvenSense-Embedded-Motion-Tracker-Driver-51.shtml 下载后分析一下 其实要移植的驱动代码就这6个文件 该驱动重点就是两个c文件：inv_mu.c和inv_mpu_dmp_motion_driver.c其中，inv_mu.c中添加了几个函数，方便读者使用。我们要做的是怎么操作上面两个文件里面的函数，方便我们得出我们想要的结果，重点介绍两个函数： 初始化DMP_Init()函数如下 void DMP_Init(void) { if(!mpu_init()) //mpu初始化 { if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //设置需要的传感器 printf("mpu_set_sensor complete ......\r\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //设置fifo printf("mpu_configure_fifo complete ......\r\n"); if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //设置采集样率 printf("mpu_set_sample_rate complete ......\r\n"); if(!dmp_load_motion_driver_firmware()) //加载dmp固件 printf("dmp_load_motion_driver_firmware complete ......\r\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) printf("dmp_set_orientation complete ......\r\n"); //设置陀螺仪方向 if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) printf("dmp_enable_feature complete ......\r\n"); if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //设置速率 printf("dmp_set_fifo_rate complete ......\r\n"); run_self_test(); //自检 if(!mpu_set_dmp_state(1)) //使能 printf("mpu_set_dmp_state complete ......\r\n"); } }DMP_Init的作用就是初始化，再设置mpu里面各种参数，然后就可以通过 Read_DMP()函数读取姿态结算数据了。 初始化Read_DMP()函数如下 uint8_t Read_DMP(float* Pitch,float* Roll,float* Yaw) { short gyro[3], accel[3], sensors; float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; unsigned long sensor_timestamp; unsigned char more; long quat[4]; if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more)) return 1; if (sensors & INV_WXYZ_QUAT) { q0=quat[0] / q30; q1=quat[1] / q30; q2=quat[2] / q30; q3=quat[3] / q30; *Pitch = (float)asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; *Roll = (float)atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll *Yaw = (float)atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; return 0; } else return 2; }ps： 使用MPU6050的DMP输出的四元数是q30格式的，也就是浮点数放大了230倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以230，然后在计算。 然后到主函数里，初始化后，直接调用Read_DMP即可。 DMP_Init(); while(1) { printf("Read_DMP Return is %d\n",Read_DMP(&Pitch,&Roll,&Yaw)); printf("Pitch is:%f,Roll is:%f,Yaw is:%f\n",Pitch,Roll,Yaw); }
还有一点 因为IIC函数我们提供，注意inv_mpu.c文件实现MPU6050和IIC接口连接的声明 #define i2c_write i2cwrite #define i2c_read i2cread #define delay_ms delay_ms #define get_ms get_ms工程文件：https://pan.baidu.com/s/1jIp5ZaY 工程环境：Keil4.7 芯片：STM32F103 PS：一年前尝试做平衡小车被各种姿态公式打败了，最近隔壁宿舍有位同学做辆平衡小车通过dmp获取欧拉角，遂再研究一下。接下来有时间的话再尝试平衡车。 本人理解不深，有问题希望能和大伙一起探讨。 |
注意：Return_DMP函数的返回值，0成功，1失败，说明不是每次都能成功读出欧拉角，所以延时一段时间后再读（比如一秒读一次数据）很有可能失败！！！【本文地址】