【多旋翼飞控】MPU6050 数据读取并进行欧拉角计算完毕

只看该作者 · 2016-1-23 17:58

本帖最后由冷不丁于 2016-1-23 17:58 编辑

写在前面：这次MPU6050数据读取的时候出现两个问题，希望大神看到能帮我分析一下原因。1，K64F官方例程，使用JLINK-SW或者CMSIS都不能在调试时进入main（）函数，断电只会在上方显示。找了很多原因，感觉不是KEIL5配置上的问题而是程序本身的问题？我的芯片选型确定是对的。
2，K64作为M4系列芯片如何打开浮点运算功能？有没有大神有程序给我一份。

MPU6050作为一款常见的，而且具有很高性价比的模块，使用起来非常方便。

MPU6050可以输出三轴加速度，三轴角速度和温度。
当然我们只需要加速度和角速度就能完成飞控姿态的识别了。
至于，加速度和角速度如何计算成欧拉角，并为我们所用，只需要跟着我的程序走一遍，你就会很清楚了。
大致算法概念是：通过I2C读取MPU6050数据->数据进行滤波和实用处理->通过四元数进行一系列可怕的tan数**算得到x,y,z的欧拉角！I2C:
PTE24 -->I2C SCL
PTE25 -->I2C SDA
串口，波特率115200：
UART TX ->PB16
UART RX ->PB17

首先是我读取MPU6050数据后进行处理后的程序效果和主要处理程序：

复制

  /*****************加速度***************************************/

                

                Accel_x=((short)((Buff[0]<<8)+Buff[1]) - 1100)*9.80/16384;

                Accel_y=((short)((Buff[2]<<8)+Buff[3]) - 1100)*9.80/16384;

                Accel_z=((short)((Buff[4]<<8)+Buff[5]) - 1100)*9.80/16384;

                                

                /*****************角速度***************************************/

                                

                Gyro_x=((short)((Buff[8]<<8)+Buff[9]) + 52)*1.0/16.4;

                Gyro_y=((short)((Buff[10]<<8)+Buff[11]) + 18)*1.0/16.4;

                Gyro_z=((short)((Buff[12]<<8)+Buff[13]) + 19)*1.0/16.4;

                                

                /*****************温度***************************************/



                Temp=((short)((Buff[6]<<8)+Buff[7]))*1.0/340+36.53;

然后，我进行数据滤波和四元素计算欧拉角后的得到了可用的数据：

处理的代码特别多，贴上来帖子就变丑了，大家自己下载吧。
int main(void)
{
hardware_init();//硬件I2C和串口初始化
Pit_Init();//定时器初始化
   OSA_Init();
MPU6050_Init();//6050初始化
Get_Offset();//校准零偏值
   PRINTF("\r\n====== START ======\r\n\r\n");
   while (1)
   {
if (true == pitIsrFlag[0])//2ms一次计算
      {
            Prepare_Data();//数据提取
Recursive_Fliter(&MPU6050_GYRO_LAST,&MPU6050_GYRO_NOW ,5);//窗口滤波
Get_Attitude(); //姿态计算
            pitIsrFlag[0] = false;
      }
      if (true == pitIsrFlag[1])//1s输出一次
      {
printf("%.3fm/s2 %.3fm/s2 %.3fm/s2 \n",Q_ANGLE.X,Q_ANGLE.Y,Q_ANGLE.Z);
            pitIsrFlag[1] = false;
      }
}
}

我debug就是进不去主函数，以前遇到过，是芯片型号选错了，这次究竟会是为什么。求大神解答，很急，在线等。
调试方式JLINK-SW。