MPU6050传感器—姿态检测

 /**************************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

 /** * @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);   
    
 }

 /** * @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
    
 }

 /** * @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)
    
 }

 /** * @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;
    
  
    
 }

   ****************************************************************************** */
    
   
    
 #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**********************/

 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]);
    
     }
    
 }

 /// 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);     //清除中断标志位
    
 	}  
    
 }

 void gyro_data_ready_cb(void)

    
 {
    
  
    
     hal.new_gyro = 1;
    
 }

 /* 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");
    
     }
    
     }
    
 }