MPU6050内部DMP固件移植解析,STM32获取欧拉角串口显示

MPU6050模块是块好东西大伙都知道,围绕这个几块钱的东西就可以做很多很好玩的东西,什么四翼飞行器、平衡车等。当然要完全使用这块模块不是那么容易。
这里写图片描述

解析说明

  • 其实我们主要是想通过6050得到欧拉角和四元数
  • 要通过6050得到四元数和欧拉角,这个过程有两种办法,一种是用原始数据(三轴加速度、三轴角速度),通过一些(卡尔曼、积分运算、减少误差零点漂移等)姿态融合运算转化即可
  • 另一种是直接用MPU6050 内部的自带的数字运动处理器(即DMP),我们要用这个DMP功能的话,就要实现加载固件
  • 那么这个DMP怎么拿呢,Invensense公司有提供了一个MPU6050的嵌入式运动驱动库,进而我们可以通过这个库移植很方便得出欧拉角
  • 不过Invensense公司提供的6050运动驱动库是基于MSP430的,需要将其移植到其他芯片上(比如:STM32F1系列),当然就要移植改代码啦

移植:

官方原版驱动下载地址:
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格式的,也就是浮点数放大了2^30倍。在换算成欧拉角之前,必须先将其转换为浮点数,也就是除以2^30,然后在计算。

然后到主函数里,初始化后,直接调用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);

    }

这里写图片描述
注意:Return_DMP函数的返回值,0成功,1失败,说明不是每次都能成功读出欧拉角,所以延时一段时间后再读(比如一秒读一次数据)很有可能失败!!!

还有一点
因为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获取欧拉角,遂再研究一下。接下来有时间的话再尝试平衡车。

本人理解不深,有问题希望能和大伙一起探讨。

©️2020 CSDN 皮肤主题: 大白 设计师: CSDN官方博客 返回首页
实付0元
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值