MPU6050传感器—姿态检测_mpu6050正方向

本节主要介绍以下内容：

姿态检测的基本概念

姿态传感器的工作原理及参数

MPU6050传感器介绍

实验：获取MPU6050原始数据

实验：移植官方DMP例程

一、姿态检测基本概念

1.1 姿态

在飞行器中，飞机姿态是非常重要的参数，以飞机自身的中心建立坐标系，当飞机绕坐标轴旋转的时候，会分别影响航偏角、横滚角及俯仰角。

那么我们检测偏航角、横滚角以及俯仰角就可以知道飞行姿态了

偏航角：飞机机头朝向偏航的角度

横滚角：飞机的机翼相对于水平面翻滚的角度

俯仰角：飞机的机头朝向的角度

假如我们知道飞机初始时是左上角的状态，只要想办法测量出基于原始状态的三个姿态角的变化量，再进行叠加，就可以获知它的实时姿态了。

1.2 坐标系

抽象来说，姿态是“载体坐标系”与“地理坐标系”之前的转换关系。

下图紫色是地理坐标系，红色是基于载体的载体坐标系，载体的姿态角就是根据载体坐标系与地理坐标系的夹角来确定的。

下面是 三种常用的坐标系：

地球坐标系：以地球球心为原点，Z轴沿地球自转轴方向，X、Y轴在赤道平面内的坐标系。

地理坐标系：它的原点在地球表面(或运载体所在的点)，Z轴沿当地地理垂线的方向(重力加速度方向)，XY轴沿当地经纬线的切线方向。根据各个轴方向的不同，可选为“东北天”（x轴指向东Y指向北）、“东南天”、“西北天”等坐标系。这是我们日常生活中使用的坐标系，平时说的东南西北方向与这个坐标系东南西北的概念一致。

载体坐标系：载体坐标系以运载体的质心为原点，一般根据运载体自身结构方向构成坐标系，如Z轴上由原点指向载体顶部，Y轴指向载体头部，X轴沿载体两侧方向。上面说基于飞机建立的坐标系就是一种载体坐标系，可类比到汽车、舰船、人体、动物或手机等各种物体。

 地理坐标系与载体坐标系都以载体为原点，所以它们可以经过简单的旋转进行转换，载体的姿态角就是根据载体坐标系与地理坐标系的夹角来确定的。

假设初始状态中，飞机的Z轴、X轴及Y轴分别与地理坐标系的天轴、北轴、东轴平行。

•当飞机绕自身的“Z”轴旋转，它会使自身的“Y”轴方向与地理坐标系的“南北”方向偏离一定角度，该角度就称为偏航角(Yaw)；

•当载体绕自身的“X”轴旋转，它会使自身的“Z”轴方向与地理坐标系的“天地”方向偏离一定角度，该角度称为俯仰角(Pitch)；

•当载体绕自身的“Y”轴旋转，它会使自身的“X”轴方向与地理坐标系的“东西”方向偏离一定角度，该角度称为横滚角。

姿态角的关系

使用陀螺仪检测角度

陀螺仪是最直观的角度检测器，它可以检测物体绕坐标轴转动的“角速度”，如同将速度对时间积分可以求出路程一样，将角速度对时间积分就可以计算出旋转的“角度”。 

陀螺仪检测的缺陷

由于陀螺仪测量角度时使用积分，会存在积分误差，若积分时间Dt越小，误差就越小。这十分容易理解，例如计算路程时，假设行车时间为1小时，我们随机选择行车过程某个时刻的速度Vt乘以1小时，求出的路程误差是极大的，因为行车的过程中并不是每个时刻都等于该时刻速度的，如果我们每5分钟检测一次车速，可得到Vt1、Vt2、Vt3-Vt12这12个时刻的车速，对各个时刻的速度乘以时间间隔(5分钟)，并对这12个结果求和，就可得出一个相对精确的行车路程了，不断提高采样频率，就可以使积分时间Dt变小，降低误差。  

同样地，提高陀螺仪传感器的采样频率，即可减少积分误差，目前非常普通的陀螺仪传感器的采样频率都可以达到8KHz，已能满足大部分应用的精度要求。

更难以解决的是器件本身误差带来的问题。例如，某种陀螺仪的误差是0.1度/秒，当陀螺仪静止不动时，理想的角速度应为0，无论它静止多久，对它进行积分测量得的旋转角度都是0，这是理想的状态；而由于存在0.1度/秒的误差，当陀螺仪静止不动时，它采样得的角速度一直为0.1度/秒，若静止了1分钟，对它进行积分测量得的旋转角度为6度，若静止了1小时，陀螺仪进行积分测量得的旋转角度就是360度，即转过了一整圈，这就变得无法忍受了。只有当正方向误差和负方向误差能正好互相抵消的时候，才能消除这种累计误差。

利用加速度计检测角度

由于直接用陀螺仪测量角度在长时间测量时会产生累计误差，因而我们又引入了检测倾角的传感器。

测量倾角最常见的例子是建筑中使用的水平仪，在重力的影响下，水平仪内的气泡能大致反映水柱所在直线与重力方向的夹角关系，利用T字型水平仪，可以检测横滚角与俯仰角，但是偏航角是无法以这样的方式检测的。

在电子设备中，一般使用加速度传感器来检测倾角，它通过检测器件在各个方向的形变情况而采样得到受力数据，根据F=ma转换，传感器直接输出加速度数据，因而被称为加速度传感器。由于地球存在重力场，所以重力在任何时刻都会作用于传感器，当传感器静止的时候(实际上加速度为0)，传感器会在该方向检测出加速度g，不能认为重力方向测出的加速度为g，就表示传感器在该方向作加速度为g的运动。

当传感器的姿态不同时，它在自身各个坐标轴检测到的重力加速度是不一样的，利用各方向的测量结果，根据力的分解原理，可求出各个坐标轴与重力之间的夹角。

因为重力方向是与地理坐标系的“天地”轴固连的，所以通过测量载体坐标系各轴与重力方向的夹角即可求得它与地理坐标系的角度旋转关系，从而获知载体姿态。

由于这种倾角检测方式是利用重力进行检测的，它无法检测到偏航角(Yaw)，原理跟T字型水平仪一样，无论如何设计水平仪，水泡都无法指示这样的角度。

另一个缺陷是加速度传感器并不会区分重力加速度与外力加速度，当物体运动的时候，它也会在运动的方向检测出加速度，特别在震动的状态下，传感器的数据会有非常大的数据变化，此时难以反应重力的实际值。

磁场检测

为了弥补加速度传感器无法检测偏航角(Yaw)的问题，我们再引入磁场检测传感器，它可以检测出各个方向上的磁场大小，通过检测地球磁场，它可实现指南针的功能，所以也被称为电子罗盘。由于地磁场与地理坐标系的“南北”轴固联，利用磁场检测传感器的指南针功能，就可以测量出偏航角(Yaw)了。

磁场检测器的缺陷

与指南针的缺陷一样，使用磁场传感器会受到外部磁场干扰，如载体本身的电磁场干扰，不同地理环境的磁铁矿干扰等等。

GPS检测

使用GPS可以直接检测出载体在地球上的坐标，假如载体在某时刻测得坐标为A，另一时刻测得坐标为B，利用两个坐标即可求出它的航向，即可以确定偏航角，且不受磁场的影响，但这种检测方式只有当载体产生大范围位移的时候才有效(GPS民用精度大概为10米级)。

姿态融合与四元数

        可以发现，使用陀螺仪检测角度时，在静止状态下存在缺陷，且受时间影响，而加速度传感器检测角度时，在运动状态下存在缺陷，且不受时间影响，刚好互补。假如我们同时使用这两种传感器，并设计一个滤波算法，当物体处于静止状态时，增大加速度数据的权重，当物体处于运动状时，增大陀螺仪数据的权重，从而获得更准确的姿态数据。

        同理，检测偏航角，当载体在静止状态时，可增大磁场检测器数据的权重，当载体在运动状态时，增大陀螺仪和GPS检测数据的权重。这些采用多种传感器数据来检测姿态的处理算法被称为姿态融合。

        在姿态融合解算的时候常常使用“四元数”来表示姿态，它由三个实数及一个虚数组成，因而被称之为四元数。使用四元数表示姿态并不直观，但因为使用欧拉角(即前面说的偏航角、横滚角及俯仰角)表示姿态的时候会有“万向节死锁”问题，且运算比较复杂，所以一般在数据处理的时候会使用四元数，处理完毕后再把四元数转换成欧拉角。

        也就是说，四元数是姿态角的另一种表示方式，感兴趣的话可自行查阅相关资料。

二、传感器工作原理

2.1 工作原理

在电子技术中，传感器一般是指把物理量转化成电信号量的装置。

        敏感元件直接感受被测物理量，并输出与该物理量有确定关系的信号，经过转换元件将该物理量信号转换为电信号，变换电路对转换元件输出的电信号进行放大调制，最后输出容易检测的电信号量。

        例如，温度传感器可把温度量转化成电压信号量输出，且温度值与电压值成比例关系，我们只要使用ADC测量出电压值，并根据转换关系即可求得实际温度值。而前文提到的陀螺仪、加速度及磁场传感器也是类似的，它们检测的角速度、加速度及磁场强度与电压值有确定的转换关系。

2.2. 传感器参数

 其中误差与分辨率是比较容易混淆的概念，以使用尺子测量长度为例，误差就是指尺子准不准，使用它测量出10厘米，与计量机构标准的10厘米有多大区别，若区别在5毫米以内，我们则称这把尺子的误差为5毫米。而分辨率是指尺子的最小刻度值，假如尺子的最小刻度值为1厘米，我们称这把尺子的分辨率为1厘米，它只能用于测量厘米级的尺寸，对于毫米级的长度，这就无法用这把尺子进行测量了。如果把尺子加热拉长，尺子的误差会大于5毫米，但它的分辨率仍为1厘米，只是它测出的1厘米值与真实值之间差得更远了。

2.3. 物理量的表示方法

大部分传感器的输出都是与电压成比例关系的，电压值一般采用ADC来测量，而ADC一般有固定的位数，如8位ADC、12位ADC等，ADC的位数会影响测量的分辨率及量程。

假设用一个2位的ADC来测量长度，2位的ADC最多只能表示0、1、2、3这四个数，假如它的分辨率为20厘米，那么它最大的测量长度为60厘米，假如它的分辨率为10厘米，那么它的最大测量长度为30厘米，由此可知，对于特定位数的ADC，量程和分辨率不可兼得。

在实际应用中，常常直接用ADC每位表征的物理量值来表示分辨率，如每位代表20厘米，我们称它的分辨率为1LSB/20cm，它等效于5位表示1米：5LSB/m。其中的LSB（Least Significant Bit），意为最ADC的低有效位。

使用采样得到的ADC数值，除以分辨率，即可求取得到物理量。例如使用分辨率为5LSB/m、线性误差为0.1m的传感器进行长度测量，其ADC采样得到数据值为“20”，可计算知道该传感器的测量值为4米，而该长度的真实值介于3.9-4.1米之间。

三、MPU6050传感器介绍

3.1 MPU6050简介

MPU6050模块，它是一种六轴传感器模块，采用InvenSense公司的MPU6050作为主芯片，能同时检测三轴加速度、三轴陀螺仪(三轴角速度)的运动数据以及温度数据。

利用MPU6050芯片内部的DMP模块（Digital Motion Processor数字运动处理器），可对传感器数据进行滤波、融合处理，它直接通过I2C接口向主控器输出姿态解算后的姿态数据，降低主控器的运算量。其姿态解算频率最高可达200Hz（利用很多个角速度的原始数据，融合加速度数据一起来算，算出来最终输出的姿态角），非常适合用于对姿态控制实时要求较高的领域。常见应用于手机、智能手环、四轴飞行器及计步器等的姿态检测。

图中表示的坐标系及旋转符号标出了MPU6050传感器的XYZ轴的加速度有角速度的正方向。 

3.2 MPU6050的特性参数

 加速度与陀螺仪传感器的ADC均为16位，它们的量程及分辨率可选多种模式：

 

从表中还可了解到传感器的加速度及陀螺仪的采样频率分别为1000Hz及8000Hz，它们是指加速度及角速度数据的采样频率，我们可以使用STM32控制器把这些数据读取出来然后进行姿态融合解算，以求出传感器当前的姿态(即求出偏航角、横滚角、俯仰角)。

而如果我们使用传感器内部的DMP单元进行解算，它可以直接对采样得到的加速度及角速度进行姿态解算，解算得到的结果再输出给STM32控制器，即STM32无需自己计算，可直接获取偏航角、横滚角及俯仰角，该DMP每秒可输出200次姿态数据。

3.3 引脚说明

该模块引出的8 个引脚功能说明见下表

        其中的SDA/SCL、XDA/XCL 通讯引脚分别为两组I2C 信号线。当模块与外部主机通讯时，使用SDA/SCL，如与STM32 芯片通讯；；而XDA/XCL 则用于MPU6050 芯片与其它I2C 传感器通讯时使用，例如使用它与磁场传感器连接，MPU6050 模块可以把从主机SDA/SCL 接收的数据或命令通过XDA/XCL 引脚转发到磁场传感器中。但实际上这种功能比较鸡肋，控制麻烦且效率低，一般会直接把磁场传感器之类的I2C 传感器直接与MPU6050 挂载在同一条总线上（即都连接到SDA/SCL），使用主机直接控制。

四、实验：获取MPU6050原始数据

4.1 硬件设计 

MPU6050 模块的硬件原理图如下：

        它的硬件非常简单， SDA 与SCL 被引出方便与外部I2C 主机连接，看图中的右上角，可知该模块的I2C 通讯引脚SDA 及SCL 已经连接了上拉电阻，因此它与外部I2C 通讯主机通讯时直接使用导线连接起来即可；而MPU6050 模块与其它传感器通讯使用的XDA、XCL 引脚没有接上拉电阻，要使用时需要注意。模块自身的I2C 设备地址可通过AD0 引脚的电平控制，当AD0 接地时，设备地址为0x68(七位地址)，当AD0 接电源时，设备地址为0x69(七位地址)。另外，当传感器有新数据的时候会通过INT 引脚通知STM32。

        由于MPU6050 检测时是基于自身中心坐标系的，它表示的坐标系及旋转符号标出了MPU6050 传感器的XYZ 轴的加速度有角速度的正方向。所以在安装模块时，需要考虑它与所在设备的坐标系统的关系。

在实验前，我们先用杜邦线把STM32 开发板与该MPU6050 模块连接起来

使用硬件I2C时不能与液晶屏同时使用，因为FSMC的NADV 与I2C1 的SDA 是同一个引脚，互相影响了，例程都默认使用软件I2C来驱动MPU6050，底层的软件I2C驱动跟EEPROM基本本一致，本章中重点讲述上层的MPU6050 应用及接口。

4.2 软件设计

4.2.1 编程要点

1. 初始化STM32的I2C
2. 使能I2C向MPU6050写入控制参数
3. 定时读取加速度、角速度及温度数据

 4.2.2 代码分析

本实验中的I2C 硬件定义见代码

/**************************I2C参数定义，I2C1或I2C2********************************/
#define             SENSORS_I2Cx                                I2C1
#define             SENSORS_I2C_APBxClock_FUN                   RCC_APB1PeriphClockCmd
#define             SENSORS_I2C_CLK                             RCC_APB1Periph_I2C1
#define             SENSORS_I2C_GPIO_APBxClock_FUN              RCC_APB2PeriphClockCmd
#define             SENSORS_I2C_GPIO_CLK                        RCC_APB2Periph_GPIOB     
#define             SENSORS_I2C_SCL_PORT                        GPIOB   
#define             SENSORS_I2C_SCL_PIN                         GPIO_Pin_6
#define             SENSORS_I2C_SDA_PORT                        GPIOB 
#define             SENSORS_I2C_SDA_PIN                         GPIO_Pin_7

这些宏根据传感器使用的I2C 硬件封装起来了

①初始化I2C

/**
  * @brief  I2C1 I/O配置
  * @param  无
  * @retval 无
  */
static void I2C_GPIO_Config(void)
{
  GPIO_InitTypeDef  GPIO_InitStructure; 
 
	/* 使能与 I2C1 有关的时钟 */
	SENSORS_I2C_APBxClock_FUN ( SENSORS_I2C_CLK, ENABLE );
	SENSORS_I2C_GPIO_APBxClock_FUN ( SENSORS_I2C_GPIO_CLK, ENABLE );
	
    
  /* PB6-I2C1_SCL、PB7-I2C1_SDA*/
  GPIO_InitStructure.GPIO_Pin = SENSORS_I2C_SCL_PIN;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;	       // 开漏输出
  GPIO_Init(SENSORS_I2C_SCL_PORT, &GPIO_InitStructure);
	
  GPIO_InitStructure.GPIO_Pin = SENSORS_I2C_SDA_PIN;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;	       // 开漏输出
  GPIO_Init(SENSORS_I2C_SDA_PORT, &GPIO_InitStructure);	
	
	
}
 
 
/**
  * @brief  I2C 工作模式配置
  * @param  无
  * @retval 无
  */
static void I2C_Mode_Configu(void)
{
  I2C_InitTypeDef  I2C_InitStructure; 
 
  /* I2C 配置 */
  I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
	
	/* 高电平数据稳定，低电平数据变化 SCL 时钟线的占空比 */
  I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
	
  I2C_InitStructure.I2C_OwnAddress1 =I2Cx_OWN_ADDRESS7; 
  I2C_InitStructure.I2C_Ack = I2C_Ack_Enable ;
	
	/* I2C的寻址模式 */
  I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
	
	/* 通信速率 */
  I2C_InitStructure.I2C_ClockSpeed = I2C_Speed;
  
	/* I2C1 初始化 */
  I2C_Init(SENSORS_I2Cx, &I2C_InitStructure);
  
	/* 使能 I2C1 */
  I2C_Cmd(SENSORS_I2Cx, ENABLE);   
}

②对读写函数的封装

初始化完成后就是编写I2C 读写函数了，这部分跟EERPOM 的一样，主要是调用STM32 标准库函数读写数据寄存器及标志位，见代码

/**
  * @brief   写数据到MPU6050寄存器
  * @param   
  * @retval  
  */
void MPU6050_WriteReg(u8 reg_add,u8 reg_dat)
{
	I2C_ByteWrite(reg_dat,reg_add); 
}
 
/**
  * @brief   从MPU6050寄存器读取数据
  * @param   
  * @retval  
  */
void MPU6050_ReadData(u8 reg_add,unsigned char* Read,u8 num)
{
	I2C_BufferRead(Read,reg_add,num);
}

/**
  * @brief   写一个字节到I2C设备中
  * @param   
  *		@arg pBuffer:缓冲区指针
  *		@arg WriteAddr:写地址 
  * @retval  正常返回1，异常返回0
  */
uint8_t I2C_ByteWrite(u8 pBuffer, u8 WriteAddr)
{
  /* Send STRAT condition */
  I2C_GenerateSTART(SENSORS_I2Cx, ENABLE);
	
	I2CTimeout = I2CT_FLAG_TIMEOUT;
 
 
  /* Test on EV5 and clear it */
  while(!I2C_CheckEvent(SENSORS_I2Cx, I2C_EVENT_MASTER_MODE_SELECT))
  {
    if((I2CTimeout--) == 0) return I2C_TIMEOUT_UserCallback();
  } 
 
  /* Send slave address for write */
  I2C_Send7bitAddress(SENSORS_I2Cx, MPU6050_SLAVE_ADDRESS, I2C_Direction_Transmitter);
  
	I2CTimeout = I2CT_FLAG_TIMEOUT;
  /* Test on EV6 and clear it */
  while(!I2C_CheckEvent(SENSORS_I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) 
	{
    if((I2CTimeout--) == 0) return I2C_TIMEOUT_UserCallback();
  }  
      
  /* Send the slave's internal address to write to */
  I2C_SendData(SENSORS_I2Cx, WriteAddr);
  
	I2CTimeout = I2CT_FLAG_TIMEOUT;
  /* Test on EV8 and clear it */
  while(!I2C_CheckEvent(SENSORS_I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTED))
	{
    if((I2CTimeout--) == 0) return I2C_TIMEOUT_UserCallback();
  } 
 
  /* Send the byte to be written */
  I2C_SendData(SENSORS_I2Cx, pBuffer); 
	
	I2CTimeout = I2CT_FLAG_TIMEOUT;
   
  /* Test on EV8 and clear it */
  while(!I2C_CheckEvent(SENSORS_I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTED))	
	{
    if((I2CTimeout--) == 0) return I2C_TIMEOUT_UserCallback();
  } 
	
  /* Send STOP condition */
  I2C_GenerateSTOP(SENSORS_I2Cx, ENABLE);
	
	return 1; //正常返回1
}
 
 
 
/**
  * @brief   从I2C设备里面读取一块数据 
  * @param   
  *		@arg pBuffer:存放从slave读取的数据的缓冲区指针
  *		@arg WriteAddr:接收数据的从设备的地址
  *     @arg NumByteToWrite:要从从设备读取的字节数
  * @retval  正常返回1，异常返回0
  */
uint8_t I2C_BufferRead(u8* pBuffer, u8 ReadAddr, u16 NumByteToRead)
{  
  I2CTimeout = I2CT_LONG_TIMEOUT;
	
  while(I2C_GetFlagStatus(SENSORS_I2Cx, I2C_FLAG_BUSY)) // Added by Najoua 27/08/2008    
  {
    if((I2CTimeout--) == 0) return I2C_TIMEOUT_UserCallback();
   }
	
  I2C_GenerateSTART(SENSORS_I2Cx, ENABLE);
  
	I2CTimeout = I2CT_FLAG_TIMEOUT;
	 
  /* Test on EV5 and clear it */
  while(!I2C_CheckEvent(SENSORS_I2Cx, I2C_EVENT_MASTER_MODE_SELECT))
	{
    if((I2CTimeout--) == 0) return I2C_TIMEOUT_UserCallback();
   }
	
  /* Send slave address for write */
  I2C_Send7bitAddress(SENSORS_I2Cx, MPU6050_SLAVE_ADDRESS, I2C_Direction_Transmitter);
 
	I2CTimeout = I2CT_FLAG_TIMEOUT;
	 
  /* Test on EV6 and clear it */
  while(!I2C_CheckEvent(SENSORS_I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) 
	{
    if((I2CTimeout--) == 0) return I2C_TIMEOUT_UserCallback();
   }
	
  /* Clear EV6 by setting again the PE bit */
  I2C_Cmd(SENSORS_I2Cx, ENABLE);
 
  /* Send the slave's internal address to write to */
  I2C_SendData(SENSORS_I2Cx, ReadAddr);  
 
	I2CTimeout = I2CT_FLAG_TIMEOUT;
	 
  /* Test on EV8 and clear it */
  while(!I2C_CheckEvent(SENSORS_I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTED))
	{
    if((I2CTimeout--) == 0) return I2C_TIMEOUT_UserCallback();
   }
	
  /* Send STRAT condition a second time */  
  I2C_GenerateSTART(SENSORS_I2Cx, ENABLE);
  
	I2CTimeout = I2CT_FLAG_TIMEOUT;
  /* Test on EV5 and clear it */
  while(!I2C_CheckEvent(SENSORS_I2Cx, I2C_EVENT_MASTER_MODE_SELECT))
	{
    if((I2CTimeout--) == 0) return I2C_TIMEOUT_UserCallback();
   }
		
  /* Send slave address for read */
  I2C_Send7bitAddress(SENSORS_I2Cx, MPU6050_SLAVE_ADDRESS, I2C_Direction_Receiver);
  
	I2CTimeout = I2CT_FLAG_TIMEOUT;
	 
  /* Test on EV6 and clear it */
  while(!I2C_CheckEvent(SENSORS_I2Cx, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED))
	{
    if((I2CTimeout--) == 0) return I2C_TIMEOUT_UserCallback();
   }
  
  /* While there is data to be read */
  while(NumByteToRead)  
  {
    if(NumByteToRead == 1)
    {
      /* Disable Acknowledgement */
      I2C_AcknowledgeConfig(SENSORS_I2Cx, DISABLE);
      
      /* Send STOP Condition */
      I2C_GenerateSTOP(SENSORS_I2Cx, ENABLE);
    }
 
    /* Test on EV7 and clear it */
    if(I2C_CheckEvent(SENSORS_I2Cx, I2C_EVENT_MASTER_BYTE_RECEIVED))  
    {      
      /* Read a byte from the slave */
      *pBuffer = I2C_ReceiveData(SENSORS_I2Cx);
 
      /* Point to the next location where the byte read will be saved */
      pBuffer++; 
      
      /* Decrement the read bytes counter */
      NumByteToRead--;        
    }   
  }
 
  /* Enable Acknowledgement to be ready for another reception */
  I2C_AcknowledgeConfig(SENSORS_I2Cx, ENABLE);
	
	return 1; //正常，返回1
}

③MPU6050 的寄存器定义

MPU6050 有各种各样的寄存器用于控制工作模式，我们把这些寄存器的地址、寄存器
位使用宏定义到了mpu6050.h 文件中了

④初始化MPU6050

根据MPU6050 的寄存器功能定义，我们使用I2C 往寄存器写入特定的控制参数

/**
  * @brief   写数据到MPU6050寄存器
  * @param   
  * @retval  
  */
void MPU6050_WriteReg(u8 reg_add,u8 reg_dat)
{
	I2C_ByteWrite(reg_dat,reg_add); 
}
 
/**
  * @brief   从MPU6050寄存器读取数据
  * @param   
  * @retval  
  */
void MPU6050_ReadData(u8 reg_add,unsigned char* Read,u8 num)
{
	I2C_BufferRead(Read,reg_add,num);
}
 
 
/**
  * @brief   初始化MPU6050芯片
  * @param   
  * @retval  
  */
void MPU6050_Init(void)
{
  int i=0,j=0;
  //在初始化之前要延时一段时间，若没有延时，则断电后再上电数据可能会出错
  for(i=0;i<1000;i++)
  {
    for(j=0;j<1000;j++)
    {
      ;
    }
  }
	MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x00);	     //解除休眠状态
	MPU6050_WriteReg(MPU6050_RA_SMPLRT_DIV , 0x07);	    //陀螺仪采样率
	MPU6050_WriteReg(MPU6050_RA_CONFIG , 0x06);	
	MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG , 0x01);	  //配置加速度传感器工作在4G模式
	MPU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x18);     //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
}

这段代码首先使用MPU6050_ReadData 及MPU6050_WriteRed 函数封装了I2C 的底层读写驱动，接下来用它们在MPU6050_Init 函数中向MPU6050 寄存器写入控制参数，设置了MPU6050 的采样率、量程(分辨率)。

 Gyroscope Output Rate / (1 + SMPLRT_DIV) = 1kHZ  设置采样频率为1Khz

 

这段代码首先使用MPU6050_ReadData 及MPU6050_WriteRed 函数封装了I2C 的底层读写驱动，接下来用它们在MPU6050_Init 函数中向MPU6050 寄存器写入控制参数，设置了MPU6050 的采样率、量程(分辨率)。

⑤读传感器ID 

初始化后，可通过读取它的“WHO AM I”寄存器内容来检测硬件是否正常，该寄存器存储了ID 号0x68，见代码

/**
  * @brief   读取MPU6050的ID
  * @param   
  * @retval  正常返回1，异常返回0
  */
uint8_t MPU6050ReadID(void)
{
	unsigned char Re = 0;
    MPU6050_ReadData(MPU6050_RA_WHO_AM_I,&Re,1);    //读器件地址
	if(Re != 0x68)
	{
		MPU_ERROR("MPU6050 dectected error!\r\n检测不到MPU6050模块，请检查模块与开发板的接线");
		return 0;
	}
	else
	{
		MPU_INFO("MPU6050 ID = %d\r\n",Re);
		return 1;
	}
		
}

⑥读取原始数据

若传感器检测正常，就可以读取它数据寄存器获取采样数据

void SysTick_Handler(void)
{
	int i;
  
  for(i=0; i<TASK_DELAY_NUM; i++)
  {
    Task_Delay_Group[i] ++;                   //任务计时，时间到后执行
  }
  
  /* 处理任务0 */
  if(Task_Delay_Group[0] >= TASK_DELAY_0)     //判断是否执行任务0
  {
    Task_Delay_Group[0] = 0;                  //置0重新计时
    
    /* 任务0：翻转LED */
    LED2_TOGGLE;
  }
  
  /* 处理任务1 */
  if(Task_Delay_Group[1] >= TASK_DELAY_1)     //判断是否执行任务1
  {
    Task_Delay_Group[1] = 0;                  //置0重新计时
    
    /* 任务1：MPU6050任务 */
    if( ! task_readdata_finish )
    {
      MPU6050ReadAcc(Acel);
      MPU6050ReadGyro(Gyro);
      MPU6050_ReturnTemp(&Temp);
      
      task_readdata_finish = 1; //标志位置1，表示需要在主循环处理MPU6050数据
    }
  }
  
  /* 处理任务2 */
  //添加任务需要修改任务总数的宏定义 TASK_DELAY_NUM
  //并且添加定义任务的执行周期宏定义 TASK_DELAY_x（x就是一个编号），比如 TASK_DELAY_2
}

/**
  * @brief   读取MPU6050的加速度数据
  * @param   
  * @retval  
  */
void MPU6050ReadAcc(short *accData)
{
    u8 buf[6];
    MPU6050_ReadData(MPU6050_ACC_OUT, buf, 6);
    accData[0] = (buf[0] << 8) | buf[1];
    accData[1] = (buf[2] << 8) | buf[3];
    accData[2] = (buf[4] << 8) | buf[5];
}
 
/**
  * @brief   读取MPU6050的角加速度数据
  * @param   
  * @retval  
  */
void MPU6050ReadGyro(short *gyroData)
{
    u8 buf[6];
    MPU6050_ReadData(MPU6050_GYRO_OUT,buf,6);
    gyroData[0] = (buf[0] << 8) | buf[1];
    gyroData[1] = (buf[2] << 8) | buf[3];
    gyroData[2] = (buf[4] << 8) | buf[5];
}
 
/**
  * @brief   读取MPU6050的原始温度数据
  * @param   
  * @retval  
  */
void MPU6050ReadTemp(short *tempData)
{
	u8 buf[2];
    MPU6050_ReadData(MPU6050_RA_TEMP_OUT_H,buf,2);     //读取温度值
    *tempData = (buf[0] << 8) | buf[1];
}
 
/**
  * @brief   读取MPU6050的温度数据，转化成摄氏度
  * @param   
  * @retval  
  */
void MPU6050_ReturnTemp(float *Temperature)
{
	short temp3;
	u8 buf[2];
	
	MPU6050_ReadData(MPU6050_RA_TEMP_OUT_H,buf,2);     //读取温度值
  temp3= (buf[0] << 8) | buf[1];	
	*Temperature=((double) temp3/340.0)+36.53;
 
}

其中前以上三个函数分别用于读取三轴加速度、角速度及温度值，这些都是原始的ADC数值(16 位长)，对于加速度和角速度，把读取得的ADC值除以分辨率，即可求得实际物理量数值。最后一个函数MPU6050_ReturnTemp 展示了温度ADC值与实际温度值间的转换，它是根据MPU6050 的说明给出的转换公式进行换算的，注意陀螺仪检测的温度会受自身芯片发热的影响，严格来说它测量的是自身芯片的温度，所以用它来测量气温是不太准确的。对于加速度和角速度值我们没有进行转换，在后面我们直接利用这些数据交给DMP 单元，求解出姿态角。

⑥main.c

  ******************************************************************************
  */
  
#include "stm32f10x.h"
#include "stm32f10x_it.h"
#include "./systick/bsp_SysTick.h"
#include "./led/bsp_led.h"
#include "./usart/bsp_usart.h"
#include "./mpu6050/mpu6050.h"
#include "./i2c/bsp_i2c.h"
 
 
/* MPU6050数据 */
short Acel[3];
short Gyro[3];
float Temp;
 
/**
  * @brief  主函数
  * @param  无  
  * @retval 无
  */
int main(void)
{
	/* LED 端口初始化 */
	LED_GPIO_Config();
	/* 串口通信初始化 */
	USART_Config();
 
	//I2C初始化
	I2C_Bus_Init();
	//MPU6050初始化
	MPU6050_Init();
  //检测MPU6050
	if( MPU6050ReadID() == 0 )
	{
		printf("\r\n没有检测到MPU6050传感器！\r\n");
		LED_RED;
		while(1);	//检测不到MPU6050 会红灯亮然后卡死
	}
	
  /* 配置SysTick定时器和中断 */
  SysTick_Init(); //配置 SysTick 为 1ms 中断一次，在中断里读取传感器数据
  SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk; //启动定时器
  
  
  while(1)
  {
    if( task_readdata_finish ) //task_readdata_finish = 1 表示读取MPU6050数据完成
    {
      
      printf("加速度：%8d%8d%8d",Acel[0],Acel[1],Acel[2]);
      
      printf("    陀螺仪%8d%8d%8d",Gyro[0],Gyro[1],Gyro[2]);
      
      printf("    温度%8.2f\r\n",Temp);
      
      task_readdata_finish = 0; // 清零标志位
      
    }
  }
  
}
/*********************************************END OF FILE**********************/

本实验中控制MPU6050 并没有使用中断检测，我们是利用Systick 定时器进行计时，隔一段时间读取MPU6050 的数据寄存器获取采样数据的，代码中使用Task_Delay 变量来控制定时时间，在Systick 中断里会每隔1ms 对该变量值减1，所以当它的值为0 时表示定时时间到。就在whlie 循环里判断定时时间，定时时间到后就读取加速度、角速度及温度值，并使用串口打印信息到电脑端。

五、MPU6050—利用DMP 进行姿态解算 

        上一小节我们仅利用MPU6050 采集了原始的数据，如果您对姿态解算的算法深有研究，可以自行编写姿态解算的算法，并利用这些数据，使用STM32 进行姿态解算，解算后输出姿态角。

        而由于MPU6050 内部集成了DMP，不需要STM32 参与解算，可直接输出姿态角，也不需要对解算算法作深入研究，非常方便，本章讲解如何使用DMP 进行解算。实验中使用的代码主体是从MPU6050 官方提供的驱动《motion_driver_6.12》移植过来的，该资料包里提供了基STM32F4 控制器的源代码（本工程正是利用该代码移植到STM32F1 上的）及使用python 语言编写的上位机，资料中还附带了说明文档，请您充分利用官方自带的资料学习。

5.1 程序设计要点

1. 提供I2C读写接口、定时服务及INT中断处理；
2. 从陀螺仪中获取原始数据并处理
3. 更新代码并输出

5.2 代码分析 

        官方驱动主要是MPL软件库（Motion Processing Library）,要移植该软件库我们需要为它提供I2C读写接口、定时服务以及MPU6050的数据更新标志。若需要输出调试信息到上位机，还需要提供串口接口。

①I2C读写接口

        MPL库的内部对I2C读写时都使用i2c_write及i2c_read函数，在文件“inv_mpu”中给出了它们的接口格式，见代码

这些接口的格式与我们上面写的I2C 读写函数Sensors_I2C_ReadRegister 及
Sensors_I2C_WriteRegister 一致，所以可直接使用宏替换。

②提供定时服务

MPL 软件库中使用到了延时及时间戳功能，要求需要提供delay_ms 函数实现毫秒级延时，提供get_ms 获取毫秒级的时间戳，它们的接口格式也在“inv_mpu.c”文件中给出，

 我们为接口提供的Delay_ms 及get_tick_count 函数定义在bsp_SysTick.c 文件，我们使用SysTick 每毫秒产生一次中断，进行计时

上述代码中的TimingDelay_Decrement 和TimeStamp_Increment 函数是在Systick 的中断服务函数中被调用的，见代码清单 50-12。systick 被配置为每毫秒产生一次中断，而每次中断中会对TimingDelay 变量减1，对g_ul_ms_ticks 变量加1。它们分别用于Delay_ms 函数利用TimingDelay 的值进行阻塞延迟，而get_tick_count 函数获取的时间戳即g_ul_ms_ticks的值。

提供串口调试接口 

MPL 代码库的调试信息输出函数都集中到了log_stm32.c 文件中，我们可以为这些函数提供串口输出接口，以便把这些信息输出到上位机。

int fputcc(int ch)
{
		/* 发送一个字节数据到USART1 */
		USART_SendData(DEBUG_USARTx, (uint8_t) ch);
		
		/* 等待发送完毕 */
		while (USART_GetFlagStatus(DEBUG_USARTx, USART_FLAG_TXE) == RESET);		
	
		return (ch);
}
 
void eMPL_send_quat(long *quat)
{
    char out[PACKET_LENGTH];
    int i;
    if (!quat)
        return;
    memset(out, 0, PACKET_LENGTH);
    out[0] = '$';
    out[1] = PACKET_QUAT;
    out[3] = (char)(quat[0] >> 24);
    out[4] = (char)(quat[0] >> 16);
    out[5] = (char)(quat[0] >> 8);
    out[6] = (char)quat[0];
    out[7] = (char)(quat[1] >> 24);
    out[8] = (char)(quat[1] >> 16);
    out[9] = (char)(quat[1] >> 8);
    out[10] = (char)quat[1];
    out[11] = (char)(quat[2] >> 24);
    out[12] = (char)(quat[2] >> 16);
    out[13] = (char)(quat[2] >> 8);
    out[14] = (char)quat[2];
    out[15] = (char)(quat[3] >> 24);
    out[16] = (char)(quat[3] >> 16);
    out[17] = (char)(quat[3] >> 8);
    out[18] = (char)quat[3];
    out[21] = '\r';
    out[22] = '\n';
		
    for (i=0; i<PACKET_LENGTH; i++) {
      fputcc(out[i]);
    }
}

上述代码中的fputcc 函数是我们自己编写的串口输出接口，它与我们重定向printf 函数定义的fputc 函数功能很类似。下面的eMPL_send_quat 函数是MPL 库中的原函数，它用于打印“四元数信息”，在这个log_stm32.c 文件中还有输出日志信息的_MLPrintLog 函数，输出原始信息到专用上位机的eMPL_send_data 函数，它们都调用了fputcc 进行输出。

MPU6050 的中断接口

与上一小节中的基础实验不同，为了高效处理采样数据，MPL代码库使用了MPU6050的INT 中断信号，为此我们要给提供中断接口

/// IO 线中断
void EXTI_INT_FUNCTION (void)
{
//	MPU_DEBUG("intterrupt");
	if(EXTI_GetITStatus(EXTI_LINE) != RESET) //确保是否产生了EXTI Line中断
	{
    LED1_TOGGLE;
		  /* Handle new gyro*/
		gyro_data_ready_cb();
		EXTI_ClearITPendingBit(EXTI_LINE);     //清除中断标志位
	}  
}

每当有新的数据产生时，本函数会被中断服务函数调用，在本工程中，它设置标志位用于指示及保护FIFO 缓冲区 

void gyro_data_ready_cb(void)
{
 
    hal.new_gyro = 1;
}

main 函数执行流程

了解MPL 移植需要提供的接口后，我们直接看main 函数了解如何利用MPL 库获取姿态数据

 

 

main 函数非常长，而且我们只是摘抄了部分，在原工程代码中还有很多代码，以及不同模式下的条件判断分支，例如加入磁场数据使用9 轴数据进行解算的功能(这是MPU9150 的功能，MPU6050 不支持)以及其它工作模式相关的控制示例。上述main 函数的主要执行流程概括如下：

1. 初始化STM32 的硬件，如Systick、LED、调试串口、INT 中断引脚以及I2C 外设的初始化；
2. 调用MPL 库函数mpu_init 初始化传感器的基本工作模式(以下过程调用的大部分都是MPL 库函数，不再强调)；
3.  调用inv_init_mpl 函数初始化MPL 软件库，初始化后才能正常进行解算；
4. 设置各种运算参数，如四元数运算(inv_enable_quaternion)、6 轴或9 轴数据融合(inv_enable_9x_sensor_fusion)等等；
5. 设置传感器的工作模式(mpu_set_sensors)、采样率(mpu_set_sample_rate)、分辨率(inv_set_gyro_orientation_and_scale)等等；
6. 当STM32 驱动、MPL 库、传感器工作模式、DMP 工作模式等所有初始化工作都完成后进行while 循环；
7. 在while 循环中检测串口的输入，若串口有输入，则调用handle_input 根据串口输入的字符(命令)，切换工作方式。这部分主要是为了支持上位机通过输入命令，根据进行不同的处理，如开、关加速度信息的采集或调试信息的输出等；
8. 在while 循环中检测是否有数据更新(if (hal.new_gyro && hal.dmp_on))，当有数据更新的时候产生INT 中断，会使hal.new_gyro 置1 的，从而执行if 里的条件代码；
9. 使用dmp_read_fifo 把数据读取到FIFO，这个FIFO 是指MPL 软件库定义的一个缓冲区，用来缓冲最新采集得的数据；
10. 调用inv_build_gyro、inv_build_temp、inv_build_accel 及inv_build_quat 函数处理数据角速度、温度、加速度及四元数数据，并对标志变量new_data 置1；
11. 在while 循环中检测new_data 标志位，当有新的数据时执行if 里的条件代码；
12. 调用inv_execute_on_data 函数更新所有数据及状态；
13. 调用read_from_mpl 函数向主机输出最新的数据。

数据输出接口

在上面main 中最后调用的read_from_mpl 函数演示了如何调用MPL 数据输出接口，通过这些接口我们可以获得想要的数据

/* Get data from MPL.
 * TODO: Add return values to the inv_get_sensor_type_xxx APIs to differentiate
 * between new and stale data.
 */
static void read_from_mpl(void)
{
    long msg, data[9];
    int8_t accuracy;
    unsigned long timestamp;
    float float_data[3] = {0};
 
    MPU_DEBUG_FUNC();
    if (inv_get_sensor_type_quat(data, &accuracy, (inv_time_t*)&timestamp)) {
       /* Sends a quaternion packet to the PC. Since this is used by the Python
        * test app to visually represent a 3D quaternion, it's sent each time
        * the MPL has new data.
        */
        eMPL_send_quat(data);
 
        /* Specific data packets can be sent or suppressed using USB commands. */
        if (hal.report & PRINT_QUAT)
            eMPL_send_data(PACKET_DATA_QUAT, data);
    }
 
    if (hal.report & PRINT_ACCEL) {
        if (inv_get_sensor_type_accel(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_ACCEL, data);
    }
    if (hal.report & PRINT_GYRO) {
        if (inv_get_sensor_type_gyro(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_GYRO, data);
 
    }
#ifdef COMPASS_ENABLED
    if (hal.report & PRINT_COMPASS) {
        if (inv_get_sensor_type_compass(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_COMPASS, data);
    }
#endif
    if (hal.report & PRINT_EULER) {
        if (inv_get_sensor_type_euler(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_EULER, data);
 
    }
		/*********发送数据到匿名四轴上位机**********/
    if(1)
    {
				#ifdef USE_LCD_DISPLAY
 
					char cStr [ 70 ];
 
				#endif
 
				unsigned long timestamp,step_count,walk_time;
 
			
				/*获取欧拉角*/
			  if (inv_get_sensor_type_euler(data, &accuracy,(inv_time_t*)&timestamp))
						{
							float Pitch,Roll,Yaw;
							Pitch =data[0]*1.0/(1<<16) ;
							Roll = data[1]*1.0/(1<<16);
							Yaw = data[2]*1.0/(1<<16);
							
							/*向匿名上位机发送姿态*/
							Data_Send_Status(Pitch,Roll,Yaw);
							/*向匿名上位机发送原始数据*/
							Send_Data((int16_t *)&sensors.gyro.raw,(int16_t *)&sensors.accel.raw);
							
							#ifdef USE_LCD_DISPLAY
 
								sprintf ( cStr, "Pitch :   %.4f   ",Pitch );	//inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.
								ILI9341_DispString_EN(30,90,cStr);
 
								sprintf ( cStr, "Roll :   %.4f   ", Roll );	//inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.
								ILI9341_DispString_EN(30,110,cStr);
 
								sprintf ( cStr, "Yaw :   %.4f   ", Yaw );	//inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.
								ILI9341_DispString_EN(30,130,cStr);
 
								/*温度*/
								mpu_get_temperature(data,(inv_time_t*)&timestamp); 
								
								sprintf ( cStr, "Temperature :   %.2f   ", data[0]*1.0/(1<<16) );	//inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.
								ILI9341_DispString_EN(30,150,cStr);
							#endif
						
						}
						
					/*获取步数*/        
				get_tick_count(&timestamp);
				if (timestamp > hal.next_pedo_ms) {
 
						hal.next_pedo_ms = timestamp + PEDO_READ_MS;
						dmp_get_pedometer_step_count(&step_count);
						dmp_get_pedometer_walk_time(&walk_time);
					
						#ifdef USE_LCD_DISPLAY
 
							sprintf(cStr, "Walked steps :  %ld  steps over  %ld  milliseconds..",step_count,walk_time);
						
							ILI9341_DispString_EN(0,180,cStr);
						#endif
 
				}
			}
 
 
    if (hal.report & PRINT_ROT_MAT) {
        if (inv_get_sensor_type_rot_mat(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_ROT, data);
    }
    if (hal.report & PRINT_HEADING) {
        if (inv_get_sensor_type_heading(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_HEADING, data);
    }
    if (hal.report & PRINT_LINEAR_ACCEL) {
        if (inv_get_sensor_type_linear_acceleration(float_data, &accuracy, (inv_time_t*)&timestamp)) {
        	MPL_LOGI("Linear Accel: %7.5f %7.5f %7.5f\r\n",
        			float_data[0], float_data[1], float_data[2]);                                        
         }
    }
    if (hal.report & PRINT_GRAVITY_VECTOR) {
            if (inv_get_sensor_type_gravity(float_data, &accuracy,
                (inv_time_t*)&timestamp))
            	MPL_LOGI("Gravity Vector: %7.5f %7.5f %7.5f\r\n",
            			float_data[0], float_data[1], float_data[2]);
    }
    if (hal.report & PRINT_PEDO) {
        unsigned long timestamp;
        get_tick_count(&timestamp);
        if (timestamp > hal.next_pedo_ms) {
            hal.next_pedo_ms = timestamp + PEDO_READ_MS;
            unsigned long step_count, walk_time;
            dmp_get_pedometer_step_count(&step_count);
            dmp_get_pedometer_walk_time(&walk_time);
            MPL_LOGI("Walked %ld steps over %ld milliseconds..\n", step_count,
            walk_time);
        }
    }
 
    /* Whenever the MPL detects a change in motion state, the application can
     * be notified. For this example, we use an LED to represent the current
     * motion state.
     */
    msg = inv_get_message_level_0(INV_MSG_MOTION_EVENT |
            INV_MSG_NO_MOTION_EVENT);
    if (msg) {
        if (msg & INV_MSG_MOTION_EVENT) {
            MPL_LOGI("Motion!\n");
        } else if (msg & INV_MSG_NO_MOTION_EVENT) {
            MPL_LOGI("No motion!\n");
        }
    }
}

​​​​​​​上述代码展示了使用inv_get_sensor_type_quat 、inv_get_sensor_type_accel 、
inv_get_sensor_type_gyro、inv_get_sensor_type_euler 及dmp_get_pedometer_step_count 函数
分别获取四元数、加速度、角速度、欧拉角及计步器数据。

代码中的eMPL_send_data 函数是使用串口按照PYTHON 上位机格式进行提交数据，上位机根据这些数据对三维模型作相应的旋转。

六、MPU6050—使用第三方上位机

是当获取得到数据后，本实验根据“匿名飞控”上位机的数据格式要求上传数据。

6.1 程序设计要点：

了解上位机的通讯协议

根据协议格式上传数据到上位机

        表中说明了两种数据帧，分别是STATUS 帧及SENSER 帧，数据帧中包含帧头、功能字、长度、主体数据及校验和。“帧头”用于表示数据包的开始，均使用两个字节的0xAA 表示；“功能字”用于区分数据帧的类型，0x01 表示STATUS 帧，0x02 表示SENSER 帧；“长度”表示后面主体数据内容的字节数；“校验和”用于校验，它是前面所有内容的和。

        其中的STATUS 帧用于向上位机传输横滚角、俯仰角及偏航角的值(100 倍)，SENSER
帧用于传输加速度、角速度及磁场强度的原始数据。 

        获取欧拉角，然后调用Data_Send_Status 格式上传到上位机，而加速度及角速度的原始数据直接从sensors 结构体变量即可获取，获取后调用Send_Data 发送出去。

利用匿名上位机显示如下：

写的比较粗糙有需要的同学可以私信我代码~