找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 7711|回复: 1
打印 上一主题 下一主题
收起左侧

基于STM8L的MPU6050数据分析(上)

[复制链接]
跳转到指定楼层
楼主
ID:140343 发表于 2016-10-9 23:48 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
MPU6050包含了加速度和角速度传感器,通过STM8L的I2C可以读取到这些加速度和角速度数据.
读取到的数据是MPU6050的内部AD在加速度和角速度的X,Y,Z轴的AD采样值.根据 RX = ADCRx采样值/灵敏度 ,可以求出各轴的实际物理值.

对于灵敏度,拿加速度计来说,如果设置MPU6050的加速度量程是+-2G,为了表示+2G~-2G这样一个范围,规定16位的寄存器最高位是符号位,表示正负,剩下的15位表示数据.因为2^15=32768,所以当寄存器中的数据为32768时,表示+2G,同样-32768表示-2G.1G应该是16384,所以+-2G的量程下,灵敏度为16384.
例如,当加速度的X轴读取到的数据是-16384,因为-16384/16384=-1,所以此时x轴的加速度为-1G.
如何根据加速度计各个轴的输出计算此时的角度呢?


如上图,加速度的y轴不动,x轴绕其旋转一定的角度a,这个角度是x轴与水平面的夹角.
上图中的g是重力加速度,当MPU6050按照上图倾斜一定角度a时,此时重力加速度在gz和gx方向上各有一个分力.根据物理中的力的分解合成,gx和gz上的分力合合成重力.
因为重力g方向与水平面垂直,所以a+b=90°
因为gz轴和gx轴垂直,所以c+b=90°
综上c=a
根据重力合成,c和d是相等的.所以角a和角d是相等的.
tan(d)=gx/gz,即tan(a)=gx/gz
为了求角a,我们可以根据mpu6050的输出数据得到gx/gz的比值,再根据数学公式中的反正切,就可以求出此时的角度值.
当加速度的x轴不动,y轴绕其旋转一定的角度a时,通过同样的方法也可以求出此时的角度.
通过上面的分析,我们可以通过mpu6050或其他一些具有加速度计的传感器,获取物体在静止时,在x轴方向和y轴方向与水平面的夹角.
之所以说是静止,是因为运动时会有运动加速度,这对角度的测量会产生干扰,所以角速度传感器测量角度会存在易受运动影响的干扰.


角速度传感器同样是为了获得角度,角速度是单位时间内旋转过的角度,注定这是一个动态的物理量,物体静止时,角速度为0.如上图,物体绕x轴方向旋转一定角度,我们把旋转这个动态过程分解,假设第1秒时,旋转到1位置,第2秒时,旋转到2位置,第2秒时,旋转到3位置.根据角速度,可以求出第1秒旋转过的角度,同样求出第2秒,第3秒的角度,将这些角度想加,就可以得到3秒的时间旋转过的角度.
本例程的总体思路是,通过STM8L读取MPU6050的加速度和角速度数据,根据加速度值,求出x方向和y方向上的角度变化,仅仅是使用了上文中提及的反正切函数.然后用匿名上位机,动态展示计算后的角度.

本例程中,对加速度和角速度数据,进行了简单的滑动窗口滤波.STM8L的主频为8MHz,I2C的通信速率为400KHz,USART通信速率为115200.
本例程的视频演示,由于只使用了加速度可以看到,角度输出并不是什么平稳,受到运动加速度的干扰.

  1. /*硬件连接*/
  2. // PC0<--->SDA   PC1---->SCL
  3. //USART1_TX on PC3 and USART1_RX on PC2
  4. /****************************************************************************************
  5. *开发环境:IAR for stm8 v6.5.3
  6. *硬件平台:STM8L-DISCOVERY
  7. *功能说明:通过硬件I2C等待的方法,
  8. *作    者:茗风
  9. ****************************************************************************************/
  10. #include"iostm8l152c6.h"
  11. #include"stdbool.h"
  12. #include"stdint.h"
  13. #include "math.h"
  14. #define x 0
  15. #define y 1
  16. #define z 2
  17. #define _gx mpu6050data_struct.gyro_data[x]
  18. #define _gy mpu6050data_struct.gyro_data[y]
  19. #define _gz mpu6050data_struct.gyro_data[z]
  20. #define _ax mpu6050data_struct.accel_data[x]
  21. #define _ay mpu6050data_struct.accel_data[y]
  22. #define _az mpu6050data_struct.accel_data[z]

  23. float Pitch=0;//
  24. float Roll=0;
  25. float Yaw=0;

  26. //Att_Angle.pit


  27. struct mpu6050data
  28. {
  29.   int16_t gyro_data[3];//MPU6050原始陀螺仪角速度值
  30.   int16_t accel_data[3];//MPU6050原始加速度计值
  31. //  int16_t temp;//温度数据
  32.   int16_t gyro_offset_data[3];//水平静止时陀螺仪偏移值
  33.   int16_t accel_offset_data[3];//水平静止时加速度计偏移值
  34. };
  35. struct mpu6050data mpu6050data_struct;

  36. bool bTX_finished=true;

  37. uint8_t ui8TX_buffer[25]={0};
  38. uint8_t ui8TX_counter=0;
  39. uint8_t ui8Read_mpu6050_buffer[14]={0};
  40. /******************************************************************************************************
  41. *  名    称:void delay_10ms(uint8_t x_ms)
  42. *  功    能:延时10ms
  43. *  入口参数:无
  44. *  出口参数:无
  45. *  说    明:
  46. *  范    例:无
  47. ******************************************************************************************************/
  48. void delay_100ms(void)
  49. {
  50.   uint8_t i,j;
  51.   for(i=0;i<255;i++)//2*255个指令周期
  52.     for(j=0;j<255;j++);//2*255个指令周期
  53.   // delay_10ms共消耗 x_ms*2*255+2*x_ms个指令周期  
  54.   // 255*2*255+2*255=130610us=130ms
  55.   // 此延时函数,延时时间为130ms
  56.   // 16M/8/2=1M 一个指令周期为1us
  57. }
  58. /******************************************************************************************************
  59. *  名    称:void GPIO_Init(void)
  60. *  功    能:初始化PC7为高速推挽输出
  61. *  入口参数:无
  62. *  出口参数:无
  63. *  说    明:
  64. *  范    例:无
  65. ******************************************************************************************************/
  66. void GPIO_Init(void)
  67. {
  68.    PC_CR1_C17  =1;//推挽输出
  69.    PC_CR2_C27  =1;//高速输出
  70.    PC_DDR_DDR7 =1;//PC7输出
  71.    PC_ODR_ODR7 =0;//输出低电平

  72.    PE_CR1_C17  =1;//推挽输出
  73.    PE_CR2_C27  =1;//高速输出
  74.    PE_DDR_DDR7 =1;//PC7输出
  75.    PE_ODR_ODR7 =0;//输出低电平
  76. }
  77. /******************************************************************************************************
  78. * 名             称:void UART_Init(void)
  79. * 功            能:UART定时器初始化系统
  80. * 入口   参数:无
  81. * 出口   参数:无
  82. * 说       明:UART初始化为 115200 1 8 n
  83. * 范      例:无
  84. ******************************************************************************************************/
  85. void UART_Init(void)
  86. {
  87.   /*********************************/
  88.   PC_DDR_DDR2 =0;//输入
  89.   PC_CR1_C12  =1;//上拉输入
  90.   PC_CR2_C22 =0;

  91.   PC_ODR_ODR3 =1;
  92.   PC_CR1_C13  =1;//推挽输出
  93.   PC_CR2_C23  =1;//输出摆率10M   
  94.   PC_DDR_DDR3 =1;//输出高电平,TX空闲状态为高电平,如果不设置,会莫名奇妙的发送0x00


  95.   CLK_PCKENR1_PCKEN15=1;//开启USART外设时钟

  96.   //开启引脚的UART功能
  97.   //00: USART1_TX on PC3 and USART1_RX on PC2
  98.   //01: USART1_TX on PA2 and USART1_RX on PA3
  99.   //10: USART1_TX on PC5 and USART1_RX on PC6
  100.   SYSCFG_RMPCR1_USART1TR_REMAP =0;//PC2,PC3
  101.   //如果需要使用其他引脚作为串口发送接收引脚,请在这里选择


  102.   //设置串口工作方式
  103.   USART1_CR1_M =0;//1 start bit,8 data bit,n stop bit
  104.   USART1_CR3_STOP0=0;USART1_CR3_STOP1=0;//1 stop bit
  105.   //设置波特率
  106.   //波特率设置为115200
  107.   // 8000000/115200=69    //8000000是此时系统的主时钟频率,4分频,16M/2=8M
  108.   //69(DEC)=0045(HEX)
  109.   USART1_BRR2 =0x05;//the BRR2 should be programmed before BRR1
  110.   USART1_BRR1 =0x04;

  111. //    USART1_CR2_TEN=0;//使能发送
  112. //    USART1_CR2_TIEN=0;//打开发送中断
  113. //    USART1_CR2_REN=1;//使能接收
  114. //    USART1_CR2_RIEN=1;//打开接收中断
  115. }
  116. /******************************************************************************************************
  117. * 名     称: IIC_init()
  118. * 功     能:初始化I2C,系统主频位4MHz,I2C通信速度位333KHz
  119. * 入口参数:无
  120. * 出口参数:无
  121. * 说    明:PC0--SDA   PC1--SCL
  122. * 范   例:无
  123. ******************************************************************************************************/
  124. void I2C_Init(void)
  125. {
  126.   //----打开IIC外设时钟----
  127.   CLK_PCKENR1_PCKEN13=1;//
  128.   I2C1_CR1_PE=0;
  129.   I2C1_CR2_ACK=1;

  130.   //----I2C输入时钟频率选择----
  131.   I2C1_FREQR_FREQ=0x08;//8MHz
  132.   /*  The allowed range is between 1 MHz and 16 MHz
  133.   000000: not allowed
  134.   000001: 1 MHz
  135.   000010: 2 MHz
  136.   ...
  137.   010000: 16 MHz                                 */

  138.   //----配置时钟控制寄存器----
  139.   I2C1_CCRH=0;
  140.   I2C1_CCRH_F_S=1; //Fast mode I2C
  141.   I2C1_CCRH_DUTY=0;
  142. /* If DUTY = 0:
  143. Period(I2C) = 3* CCR * tMASTER
  144. thigh = CCR * tMASTER
  145. tlow = 2 * CCR * tMASTER*/
  146.   I2C1_CCRL=7;    //SCL高电平时间配置
  147.   //I2C的SCK时钟设置为400KHz,则SCK周期为2.5us   2.5us/0.125/3=7   
  148.   //因为I2C1_FREQR_FREQ=0x08,即I2C输入时钟频率为8M,周期为0.125us  
  149.   //CCR=7时,SCK的低电平时间为2*tlow=2*7*0.125us=1.75us,SCk高电平时间为thigh=7*0.125us=0.875us
  150.   //所以CCR=7时,SCK输出频率为380KHz

  151.   //----配置上升时间寄存器----
  152.   I2C1_TRISER_TRISE=5;//in standard mode, the maximum allowed SCL rise time is 1000 ns.
  153.   //1 us / 0.125 us = 8
  154.   //+1
  155.   I2C1_CR1_PE=1;//

  156. }
  157. /******************************************************************************************************
  158. * 名 称: uint8_t I2C_ReadOneByteDataFromSlave(uint8_t address)
  159. * 功 能:从I2C从设备中读取一字节的数据
  160. * 入口参数:address:读取数据的寄存器地址
  161. * 出口参数:返回一个从I2C从设备指定地址读到的数据
  162. * 说 明:
  163. * 范 例:无
  164. ******************************************************************************************************/
  165. uint8_t I2C_ReadOneByteDataFromSlave(uint8_t address)
  166. {
  167.   volatile uint8_t t;
  168.   //----------I2C起始信号--------------
  169.   I2C1_CR2_START=1;//产生一个起始条件
  170.   while(!(I2C1_SR1_SB==1));//读SR1寄存器,清除SB标志位
  171. //  _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时

  172.   //-------发送写I2C从器件地址---------
  173.   I2C1_DR=0xD0;//发送从设备地址
  174.   while(!(I2C1_SR1_ADDR==1));//读SR1寄存器,清除ADDR标志位
  175. // _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
  176.   if(I2C1_SR3_TRA==0)return 1;//读SR3寄存器,清除ADDR标志位
  177.   //  0: Data bytes received
  178.   //  1: Data bytes transmitted

  179.   //-----写I2C从器件寄存器地址--------
  180.   I2C1_DR=address;
  181.   while(!(I2C1_SR1_BTF==1));//等待地址发送完成
  182. //  _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时

  183.   //--------I2C重复起始信号-----------
  184.   I2C1_CR2_START=1;//重复产生一个起始条件
  185.   while(!(I2C1_SR1_SB==1));//读SR1寄存器,清除SB标志位
  186. //  _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时

  187.   //-------发送读I2C从器件地址---------
  188.   I2C1_DR=0xD1;//发送从设备地址
  189.   while(!(I2C1_SR1_ADDR==1));//读SR1寄存器,清除ADDR标志位
  190. //  _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
  191.   if(I2C1_SR3_TRA==1)return 1;//读SR3寄存器,清除ADDR标志位

  192.   //-------------停止信号-------------
  193.   I2C1_CR2_ACK=0;//ACK位控制着ACK信号,此位为0可以产生一个NOACK信号
  194.   I2C1_CR2_STOP=1;

  195.   //-------------等待接收到数据-------------
  196.   while(!(I2C1_SR1_RXNE==1));//等待地址发送完成

  197.   //-------------读取数据-------------
  198.   t=I2C1_DR;

  199.   return t;
  200. }
  201. /******************************************************************************************************
  202. * 名 称:void I2C_WriteOneByteDataToSlave(uint8_t address,uint8_t dat)
  203. * 功 能:写入一字节的数据到I2C设备中
  204. * 入口参数:address:写入的数据存储地址    dat:待写入的数据
  205. * 出口参数:无
  206. * 说 明: 通过MSTM8L硬件写入I2C设备一个字节的数据
  207. * 范 例:无
  208. ******************************************************************************************************/
  209. uint8_t I2C_WriteOneByteDataToSlave(uint8_t address,uint8_t dat)
  210. {
  211.   volatile uint8_t t;
  212.   I2C1_CR2_ACK=1;
  213.   //----------I2C起始信号--------------
  214.   I2C1_CR2_START=1;//产生一个起始条件
  215.   while(!(I2C1_SR1_SB==1));
  216. // _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
  217.   I2C1_DR=0xD0;

  218.   //--------写I2C从器件地址-----------
  219.   while(!(I2C1_SR1_ADDR==1));
  220. //  _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
  221.   if(I2C1_SR3_TRA==0)return 1;//读SR3寄存器,清除ADDR标志位

  222.   //-----写I2C从器件寄存器地址--------
  223.   while(!(I2C1_SR1_TXE==1));
  224.   I2C1_DR=address;

  225.   //-------写I2C数据到寄存器中--------
  226.   while(!(I2C1_SR1_TXE==1));
  227.   I2C1_DR=dat;
  228.   while(!(I2C1_SR1_TXE==1));
  229.   while(!(I2C1_SR1_BTF==1));
  230. // _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时

  231.   //-------------停止信号-------------
  232.   I2C1_CR2_STOP=1;
  233.   return 0;
  234. }
  235. /******************************************************************************************************
  236. * 功    能:从I2C从设备读取多个字节数据
  237. * 入口函数:
  238. * 出口函数:
  239. * 说    明:
  240. * 范    例:
  241. * 日    期:
  242. ******************************************************************************************************/
  243. uint8_t I2C_ReadMultiBytesFromSlave(uint8_t address,uint8_t *rxbuf,uint8_t len)
  244. {
  245.   volatile uint8_t i=0;

  246.   if(len==0)return 1;//如果写入字节长度为0退出

  247.   I2C1_CR2_ACK=1;
  248.   //----------I2C起始信号--------------
  249.   I2C1_CR2_START=1;//产生一个起始条件
  250.   while(!(I2C1_SR1_SB==1));//读SR1寄存器,清除SB标志位

  251.   //-------发送写I2C从器件地址---------
  252.   I2C1_DR=0xD0;//发送从设备地址
  253.   while(!(I2C1_SR1_ADDR==1));//读SR1寄存器,清除ADDR标志位
  254.   if(I2C1_SR3_TRA==0)return 1;//读SR3寄存器,清除ADDR标志位
  255.   //  0: Data bytes received
  256.   //  1: Data bytes transmitted

  257.   //-----写I2C从器件寄存器地址--------
  258.   I2C1_DR=address;
  259.   while(!(I2C1_SR1_BTF==1));//等待地址发送完成

  260.   //--------I2C重复起始信号-----------
  261.   I2C1_CR2_START=1;//重复产生一个起始条件
  262.   while(!(I2C1_SR1_SB==1));//读SR1寄存器,清除SB标志位

  263.   //-------发送读I2C从器件地址---------
  264.   I2C1_DR=0xD1;//发送从设备地址
  265.   while(!(I2C1_SR1_ADDR==1));//读SR1寄存器,清除ADDR标志位
  266.   if(I2C1_SR3_TRA==1)return 1;//读SR3寄存器,清除ADDR标志位
  267.   //-------------读取数据-------------
  268.   if(len>1)
  269.   {
  270.       for( i=len;i>1;i-- )
  271.       {
  272.         while(!(I2C1_SR1_RXNE==1));//等待I2C1_DR接收到数
  273.         *rxbuf++ = I2C1_DR;  
  274.       }
  275.   }

  276.   //-------------停止信号-------------
  277.   I2C1_CR2_ACK=0;//ACK位控制着ACK信号,此位为0可以产生一个NOACK信号
  278.   I2C1_CR2_STOP=1;

  279.   while(!(I2C1_SR1_RXNE==1));//等待I2C1_DR接收到数
  280.   *rxbuf++ = I2C1_DR;

  281.   return 0;
  282. }
  283. /******************************************************************************************************
  284. * 功    能:MPU6050初始化
  285. * 入口函数:
  286. * 出口函数:
  287. * 说    明:
  288. * 范    例:
  289. * 日    期:
  290. ******************************************************************************************************/
  291. void MPU6050_Config(void)
  292. {
  293.   I2C_WriteOneByteDataToSlave(0x6B,0x02);//电源管理,唤醒MPU6050,时钟源选择:Y轴陀螺仪时钟倍频
  294. //  I2C_WriteOneByteDataToSlave(0x6B,0x00);//电源管理,唤醒MPU6050,时钟源选择:Y轴陀螺仪时钟倍频
  295.   I2C_WriteOneByteDataToSlave(0x1B,0x18);//陀螺仪最大量程 +-2000度每秒
  296.   I2C_WriteOneByteDataToSlave(0x1C,0x1F);//加速度度最大量程 +-2G

  297.   I2C_WriteOneByteDataToSlave(0x19,0x00);//陀螺仪采样率,不分频,8khz
  298.   I2C_WriteOneByteDataToSlave(0x1A,0x00);//不启用低通滤波器
  299. //  I2C_WriteOneByteDataToSlave(0x1A,0x01);//使用数字低通滤波器
  300. }
  301. /******************************************************************************************************
  302. * 功    能:该函数连续读取,MPU6050输出的加速度和角速度数据
  303. * 入口函数:a为存放读取到数据的数组名
  304. * 出口函数:
  305. * 说    明:在mpu6050.h头文件中宏定义了这个读取mpu6050数据的函数,一行不足写函数,所以进行了宏定义
  306. * 范    例:
  307. * 日    期:
  308. ******************************************************************************************************/
  309. #define Read_Data_form_MPU6050(a)  I2C_ReadMultiBytesFromSlave(0x3B,a,14)//读取mpu6050输出的加速度和角速度数据
  310. /******************************************************************************************************
  311. * 功    能:
  312. * 入口函数:
  313. * 出口函数:
  314. * 说    明:将读取到的高8位和低8位数据合并成一个完整的16位数据
  315. * 范    例:
  316. * 日    期:
  317. ******************************************************************************************************/
  318. #define ACC_N   10
  319. #define GYRO_N  2
  320. void MPU6050_Data_Compose(uint8_t *buf)
  321. {
  322.   static uint8_t  acc_cnts=0,gyro_cnts=0;
  323.   static int16_t Data_ACC[3][ACC_N]={0};
  324.   static int16_t Data_GYRO[3][GYRO_N]={0};
  325.   static int32_t sum_acc_x=0,sum_acc_y=0,sum_acc_z=0;
  326.   static int32_t sum_gyro_x=0,sum_gyro_y=0,sum_gyro_z=0;
  327.   //----合成加速度和角速度数据----        
  328.         mpu6050data_struct.accel_data[x]=((int16_t)buf[0]<<8) + buf[1];// - mpu6050data_struct.accel_offset_data[x] ;
  329.         mpu6050data_struct.accel_data[y]=((int16_t)buf[2]<<8) + buf[3];// - mpu6050data_struct.accel_offset_data[y];
  330.         mpu6050data_struct.accel_data[z]=((int16_t)buf[4]<<8) + buf[5];// - mpu6050data_struct.accel_offset_data[z];

  331.         mpu6050data_struct.gyro_data[x] = ((int16_t)buf[8]<<8) + buf[9];// - mpu6050data_struct.gyro_offset_data[x];
  332.         mpu6050data_struct.gyro_data[y] = ((int16_t)buf[10]<<8) + buf[11];// - mpu6050data_struct.gyro_offset_data[y];
  333.         mpu6050data_struct.gyro_data[z] = ((int16_t)buf[12]<<8) + buf[13];// - mpu6050data_struct.gyro_offset_data[z];
  334.   //----对加速度进行滑动窗口滤波----        
  335.   sum_acc_x-=Data_ACC[x][acc_cnts]; //减去最旧的一个数据
  336.   sum_acc_y-=Data_ACC[y][acc_cnts];
  337.   sum_acc_z-=Data_ACC[z][acc_cnts];
  338.   Data_ACC[x][acc_cnts]=mpu6050data_struct.accel_data[x];//减去最旧的一个数据
  339.   Data_ACC[y][acc_cnts]=mpu6050data_struct.accel_data[y];
  340.   Data_ACC[z][acc_cnts]=mpu6050data_struct.accel_data[z];
  341.   sum_acc_x+=Data_ACC[x][acc_cnts];
  342.   sum_acc_y+=Data_ACC[y][acc_cnts];
  343.   sum_acc_z+=Data_ACC[z][acc_cnts];
  344.   acc_cnts++;
  345.   if(acc_cnts==ACC_N)acc_cnts=0;
  346.   mpu6050data_struct.accel_data[x]=sum_acc_x/ACC_N;//求平均值
  347.   mpu6050data_struct.accel_data[y]=sum_acc_y/ACC_N;
  348.   mpu6050data_struct.accel_data[z]=sum_acc_z/ACC_N;
  349.   //----对角速度进行滑动窗口滤波----        
  350.   sum_gyro_x-=Data_GYRO[x][gyro_cnts];//减去最旧的一个数据
  351.   sum_gyro_y-=Data_GYRO[y][gyro_cnts];
  352.   sum_gyro_z-=Data_GYRO[z][gyro_cnts];
  353.   Data_GYRO[x][gyro_cnts]=mpu6050data_struct.gyro_data[x];
  354.   Data_GYRO[y][gyro_cnts]=mpu6050data_struct.gyro_data[y];
  355.   Data_GYRO[z][gyro_cnts]=mpu6050data_struct.gyro_data[z];
  356.   sum_gyro_x+=Data_GYRO[x][gyro_cnts];//加上最新的数据
  357.   sum_gyro_y+=Data_GYRO[y][gyro_cnts];
  358.   sum_gyro_z+=Data_GYRO[z][gyro_cnts];
  359.   gyro_cnts++;
  360.   if(gyro_cnts==GYRO_N)gyro_cnts=0;
  361.   mpu6050data_struct.gyro_data[x]=sum_gyro_x/GYRO_N;//求平均值
  362.   mpu6050data_struct.gyro_data[y]=sum_gyro_y/GYRO_N;
  363.   mpu6050data_struct.gyro_data[z]=sum_gyro_z/GYRO_N;  
  364. }
  365. /******************************************************************************************************
  366. * 功    能:
  367. * 入口函数:
  368. * 出口函数:
  369. * 说    明:
  370. * 范    例:
  371. * 日    期:
  372. ******************************************************************************************************/
  373. void Prepare_Data(void)
  374. {
  375.   Read_Data_form_MPU6050(ui8Read_mpu6050_buffer);
  376.   MPU6050_Data_Compose(ui8Read_mpu6050_buffer);        
  377. }

  378. /******************************************************************************************************
  379. * 功    能:发送飞行姿态数据给匿名上位机
  380. * 入口函数:
  381. * 出口函数:
  382. * 说    明:根据匿名上位机协议发送数据
  383. * 范    例:
  384. * 日    期:
  385. ******************************************************************************************************/
  386. void SendDataToStatus(void)
  387. {
  388.         int16_t _tmp;
  389.         uint8_t sum = 0,i=0;
  390.         ui8TX_buffer[0]=0xAA;
  391.         ui8TX_buffer[1]=0xAA;
  392.         ui8TX_buffer[2]=0x01;
  393.         ui8TX_buffer[3]=12;
  394.         
  395.         _tmp = (int16_t)(Roll*100);//翻滚脚
  396.         ui8TX_buffer[4] = (_tmp&0xFF00)>>8;//acc
  397.         ui8TX_buffer[5] = _tmp&0x00FF;
  398.         
  399.         _tmp = (int16_t)(Pitch*100);//俯仰角
  400.         ui8TX_buffer[6] = (_tmp&0xFF00)>>8;
  401.         ui8TX_buffer[7] = _tmp&0x00FF;
  402.         
  403.   _tmp = (int16_t)(Yaw*100);//偏航角
  404.         ui8TX_buffer[8] = (_tmp&0xFF00)>>8;
  405.         ui8TX_buffer[9] = _tmp&0x00FF;
  406.         
  407.         ui8TX_buffer[10]=0;//没有超声波,只好发送无效数据
  408.         ui8TX_buffer[11]=0;
  409.         ui8TX_buffer[12]=0;
  410.         ui8TX_buffer[13]=0;
  411.         ui8TX_buffer[14]=0;//飞行模式
  412.         ui8TX_buffer[15]=0;//解锁/加锁
  413.         
  414.         for(i=0;i<16;i++)
  415.                 sum += ui8TX_buffer[i];
  416.                
  417.         ui8TX_buffer[16] = sum;
  418.         ui8TX_counter=17;
  419.   PC_ODR_ODR7 ^=0x01;
  420.   USART1_CR2_TEN=1;//打开发送
  421.   USART1_CR2_TIEN=1;//打开发送中断
  422. }

  423. void main(void)
  424. {
  425.   delay_100ms();
  426.   CLK_CKDIVR =0x01;//8M
  427.   GPIO_Init();
  428.   UART_Init();
  429.   I2C_Init();
  430.   MPU6050_Config();
  431.   asm("rim");               //enable interrupts
  432.   while(1)
  433.   {
  434.     if(true==bTX_finished)
  435.           {
  436.       bTX_finished=false;
  437.       Prepare_Data();    //读取MPU6050原始数据,并对数据进行简单滤波

  438.       Pitch=atan((float)_ax/(float)_az)*57.3;  //y轴固定,x轴方向与水平方向夹角
  439.       Roll=atan((float)_ay/(float)_az)*57.3;  //x轴固定,y轴方向与水平方向夹角
  440.       SendDataToStatus();//发送姿态数据到上位机
  441.           }
  442. //    asm("wfi");
  443.   }
  444. }
  445. /*************************************************************************************************** ***
  446. * 名 称: UART_OnTx()
  447. * 功 能: UART的Tx事件处理函数
  448. * 入口参数:无
  449. * 出口参数:无
  450. * 说 明:发送ui8TX_buffer数组中的数据,发送完成后,置位bTX_finished标志位,下一次进入中断后关闭发送
  451. * 范 例:无
  452. ******************************************************************************************************/
  453. #pragma vector=USART_T_TXE_vector
  454. __interrupt void USART_TX_ISR (void)
  455. {
  456.   static uint8_t cnts=0;
  457.   static bool    bSend_EndByte=false;//发送最后一字节数据
  458.   if(bSend_EndByte && USART1_SR_TC==1)
  459.   {
  460.     USART1_CR2_TIEN=0;//关闭发送中断
  461.     USART1_CR2_TEN=0;//关闭发送
  462.           bSend_EndByte=false;
  463.           bTX_finished=true;
  464.   }
  465.   else if(USART1_SR_TC==1)
  466.   {
  467.     USART1_DR=ui8TX_buffer[cnts++];
  468.     if(cnts==ui8TX_counter)
  469.     {
  470.       cnts=0;
  471.       bSend_EndByte=true;
  472.     }
  473.   }
  474. }
  475. /******************************************************************************************************
  476. * 名               称: UART_OnRx()
  477. * 功         能: UART的Tx事件处理函数
  478. * 入口           参数:无
  479. * 出口           参数:无
  480. * 说               明:
  481. * 范              例:无
  482. ******************************************************************************************************/
  483. //#pragma vector=USART_R_RXNE_vector
  484. //__interrupt void USART_RX_ISR(void)
  485. //{
  486. //  static uint8_t ui8RX_temp=0;
  487. //  if(USART1_SR_RXNE==1)
  488. //  {
  489. //    ui8RX_temp = USART1_DR;
  490. //  }
  491. //}
复制代码
分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏2 分享淘帖 顶 踩
回复

使用道具 举报

沙发
ID:221145 发表于 2017-10-16 17:36 | 只看该作者
楼主牛逼,下在那里,没看到啊~~·
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表