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.
本例程的视频演示,由于只使用了加速度可以看到,角度输出并不是什么平稳,受到运动加速度的干扰.
- /*硬件连接*/
- // PC0<--->SDA PC1---->SCL
- //USART1_TX on PC3 and USART1_RX on PC2
- /****************************************************************************************
- *开发环境:IAR for stm8 v6.5.3
- *硬件平台:STM8L-DISCOVERY
- *功能说明:通过硬件I2C等待的方法,
- *作 者:茗风
- ****************************************************************************************/
- #include"iostm8l152c6.h"
- #include"stdbool.h"
- #include"stdint.h"
- #include "math.h"
- #define x 0
- #define y 1
- #define z 2
- #define _gx mpu6050data_struct.gyro_data[x]
- #define _gy mpu6050data_struct.gyro_data[y]
- #define _gz mpu6050data_struct.gyro_data[z]
- #define _ax mpu6050data_struct.accel_data[x]
- #define _ay mpu6050data_struct.accel_data[y]
- #define _az mpu6050data_struct.accel_data[z]
- float Pitch=0;//
- float Roll=0;
- float Yaw=0;
- //Att_Angle.pit
- struct mpu6050data
- {
- int16_t gyro_data[3];//MPU6050原始陀螺仪角速度值
- int16_t accel_data[3];//MPU6050原始加速度计值
- // int16_t temp;//温度数据
- int16_t gyro_offset_data[3];//水平静止时陀螺仪偏移值
- int16_t accel_offset_data[3];//水平静止时加速度计偏移值
- };
- struct mpu6050data mpu6050data_struct;
- bool bTX_finished=true;
- uint8_t ui8TX_buffer[25]={0};
- uint8_t ui8TX_counter=0;
- uint8_t ui8Read_mpu6050_buffer[14]={0};
- /******************************************************************************************************
- * 名 称:void delay_10ms(uint8_t x_ms)
- * 功 能:延时10ms
- * 入口参数:无
- * 出口参数:无
- * 说 明:
- * 范 例:无
- ******************************************************************************************************/
- void delay_100ms(void)
- {
- uint8_t i,j;
- for(i=0;i<255;i++)//2*255个指令周期
- for(j=0;j<255;j++);//2*255个指令周期
- // delay_10ms共消耗 x_ms*2*255+2*x_ms个指令周期
- // 255*2*255+2*255=130610us=130ms
- // 此延时函数,延时时间为130ms
- // 16M/8/2=1M 一个指令周期为1us
- }
- /******************************************************************************************************
- * 名 称:void GPIO_Init(void)
- * 功 能:初始化PC7为高速推挽输出
- * 入口参数:无
- * 出口参数:无
- * 说 明:
- * 范 例:无
- ******************************************************************************************************/
- void GPIO_Init(void)
- {
- PC_CR1_C17 =1;//推挽输出
- PC_CR2_C27 =1;//高速输出
- PC_DDR_DDR7 =1;//PC7输出
- PC_ODR_ODR7 =0;//输出低电平
- PE_CR1_C17 =1;//推挽输出
- PE_CR2_C27 =1;//高速输出
- PE_DDR_DDR7 =1;//PC7输出
- PE_ODR_ODR7 =0;//输出低电平
- }
- /******************************************************************************************************
- * 名 称:void UART_Init(void)
- * 功 能:UART定时器初始化系统
- * 入口 参数:无
- * 出口 参数:无
- * 说 明:UART初始化为 115200 1 8 n
- * 范 例:无
- ******************************************************************************************************/
- void UART_Init(void)
- {
- /*********************************/
- PC_DDR_DDR2 =0;//输入
- PC_CR1_C12 =1;//上拉输入
- PC_CR2_C22 =0;
- PC_ODR_ODR3 =1;
- PC_CR1_C13 =1;//推挽输出
- PC_CR2_C23 =1;//输出摆率10M
- PC_DDR_DDR3 =1;//输出高电平,TX空闲状态为高电平,如果不设置,会莫名奇妙的发送0x00
- CLK_PCKENR1_PCKEN15=1;//开启USART外设时钟
- //开启引脚的UART功能
- //00: USART1_TX on PC3 and USART1_RX on PC2
- //01: USART1_TX on PA2 and USART1_RX on PA3
- //10: USART1_TX on PC5 and USART1_RX on PC6
- SYSCFG_RMPCR1_USART1TR_REMAP =0;//PC2,PC3
- //如果需要使用其他引脚作为串口发送接收引脚,请在这里选择
- //设置串口工作方式
- USART1_CR1_M =0;//1 start bit,8 data bit,n stop bit
- USART1_CR3_STOP0=0;USART1_CR3_STOP1=0;//1 stop bit
- //设置波特率
- //波特率设置为115200
- // 8000000/115200=69 //8000000是此时系统的主时钟频率,4分频,16M/2=8M
- //69(DEC)=0045(HEX)
- USART1_BRR2 =0x05;//the BRR2 should be programmed before BRR1
- USART1_BRR1 =0x04;
- // USART1_CR2_TEN=0;//使能发送
- // USART1_CR2_TIEN=0;//打开发送中断
- // USART1_CR2_REN=1;//使能接收
- // USART1_CR2_RIEN=1;//打开接收中断
- }
- /******************************************************************************************************
- * 名 称: IIC_init()
- * 功 能:初始化I2C,系统主频位4MHz,I2C通信速度位333KHz
- * 入口参数:无
- * 出口参数:无
- * 说 明:PC0--SDA PC1--SCL
- * 范 例:无
- ******************************************************************************************************/
- void I2C_Init(void)
- {
- //----打开IIC外设时钟----
- CLK_PCKENR1_PCKEN13=1;//
- I2C1_CR1_PE=0;
- I2C1_CR2_ACK=1;
- //----I2C输入时钟频率选择----
- I2C1_FREQR_FREQ=0x08;//8MHz
- /* The allowed range is between 1 MHz and 16 MHz
- 000000: not allowed
- 000001: 1 MHz
- 000010: 2 MHz
- ...
- 010000: 16 MHz */
- //----配置时钟控制寄存器----
- I2C1_CCRH=0;
- I2C1_CCRH_F_S=1; //Fast mode I2C
- I2C1_CCRH_DUTY=0;
- /* If DUTY = 0:
- Period(I2C) = 3* CCR * tMASTER
- thigh = CCR * tMASTER
- tlow = 2 * CCR * tMASTER*/
- I2C1_CCRL=7; //SCL高电平时间配置
- //I2C的SCK时钟设置为400KHz,则SCK周期为2.5us 2.5us/0.125/3=7
- //因为I2C1_FREQR_FREQ=0x08,即I2C输入时钟频率为8M,周期为0.125us
- //CCR=7时,SCK的低电平时间为2*tlow=2*7*0.125us=1.75us,SCk高电平时间为thigh=7*0.125us=0.875us
- //所以CCR=7时,SCK输出频率为380KHz
- //----配置上升时间寄存器----
- I2C1_TRISER_TRISE=5;//in standard mode, the maximum allowed SCL rise time is 1000 ns.
- //1 us / 0.125 us = 8
- //+1
- I2C1_CR1_PE=1;//
- }
- /******************************************************************************************************
- * 名 称: uint8_t I2C_ReadOneByteDataFromSlave(uint8_t address)
- * 功 能:从I2C从设备中读取一字节的数据
- * 入口参数:address:读取数据的寄存器地址
- * 出口参数:返回一个从I2C从设备指定地址读到的数据
- * 说 明:
- * 范 例:无
- ******************************************************************************************************/
- uint8_t I2C_ReadOneByteDataFromSlave(uint8_t address)
- {
- volatile uint8_t t;
- //----------I2C起始信号--------------
- I2C1_CR2_START=1;//产生一个起始条件
- while(!(I2C1_SR1_SB==1));//读SR1寄存器,清除SB标志位
- // _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
- //-------发送写I2C从器件地址---------
- I2C1_DR=0xD0;//发送从设备地址
- while(!(I2C1_SR1_ADDR==1));//读SR1寄存器,清除ADDR标志位
- // _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
- if(I2C1_SR3_TRA==0)return 1;//读SR3寄存器,清除ADDR标志位
- // 0: Data bytes received
- // 1: Data bytes transmitted
- //-----写I2C从器件寄存器地址--------
- I2C1_DR=address;
- while(!(I2C1_SR1_BTF==1));//等待地址发送完成
- // _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
- //--------I2C重复起始信号-----------
- I2C1_CR2_START=1;//重复产生一个起始条件
- while(!(I2C1_SR1_SB==1));//读SR1寄存器,清除SB标志位
- // _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
- //-------发送读I2C从器件地址---------
- I2C1_DR=0xD1;//发送从设备地址
- while(!(I2C1_SR1_ADDR==1));//读SR1寄存器,清除ADDR标志位
- // _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
- if(I2C1_SR3_TRA==1)return 1;//读SR3寄存器,清除ADDR标志位
- //-------------停止信号-------------
- I2C1_CR2_ACK=0;//ACK位控制着ACK信号,此位为0可以产生一个NOACK信号
- I2C1_CR2_STOP=1;
- //-------------等待接收到数据-------------
- while(!(I2C1_SR1_RXNE==1));//等待地址发送完成
- //-------------读取数据-------------
- t=I2C1_DR;
- return t;
- }
- /******************************************************************************************************
- * 名 称:void I2C_WriteOneByteDataToSlave(uint8_t address,uint8_t dat)
- * 功 能:写入一字节的数据到I2C设备中
- * 入口参数:address:写入的数据存储地址 dat:待写入的数据
- * 出口参数:无
- * 说 明: 通过MSTM8L硬件写入I2C设备一个字节的数据
- * 范 例:无
- ******************************************************************************************************/
- uint8_t I2C_WriteOneByteDataToSlave(uint8_t address,uint8_t dat)
- {
- volatile uint8_t t;
- I2C1_CR2_ACK=1;
- //----------I2C起始信号--------------
- I2C1_CR2_START=1;//产生一个起始条件
- while(!(I2C1_SR1_SB==1));
- // _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
- I2C1_DR=0xD0;
- //--------写I2C从器件地址-----------
- while(!(I2C1_SR1_ADDR==1));
- // _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
- if(I2C1_SR3_TRA==0)return 1;//读SR3寄存器,清除ADDR标志位
- //-----写I2C从器件寄存器地址--------
- while(!(I2C1_SR1_TXE==1));
- I2C1_DR=address;
- //-------写I2C数据到寄存器中--------
- while(!(I2C1_SR1_TXE==1));
- I2C1_DR=dat;
- while(!(I2C1_SR1_TXE==1));
- while(!(I2C1_SR1_BTF==1));
- // _5NOPS;//根据数据手册,检测到标志位后,需插入5个NOP进行延时
- //-------------停止信号-------------
- I2C1_CR2_STOP=1;
- return 0;
- }
- /******************************************************************************************************
- * 功 能:从I2C从设备读取多个字节数据
- * 入口函数:
- * 出口函数:
- * 说 明:
- * 范 例:
- * 日 期:
- ******************************************************************************************************/
- uint8_t I2C_ReadMultiBytesFromSlave(uint8_t address,uint8_t *rxbuf,uint8_t len)
- {
- volatile uint8_t i=0;
- if(len==0)return 1;//如果写入字节长度为0退出
- I2C1_CR2_ACK=1;
- //----------I2C起始信号--------------
- I2C1_CR2_START=1;//产生一个起始条件
- while(!(I2C1_SR1_SB==1));//读SR1寄存器,清除SB标志位
- //-------发送写I2C从器件地址---------
- I2C1_DR=0xD0;//发送从设备地址
- while(!(I2C1_SR1_ADDR==1));//读SR1寄存器,清除ADDR标志位
- if(I2C1_SR3_TRA==0)return 1;//读SR3寄存器,清除ADDR标志位
- // 0: Data bytes received
- // 1: Data bytes transmitted
- //-----写I2C从器件寄存器地址--------
- I2C1_DR=address;
- while(!(I2C1_SR1_BTF==1));//等待地址发送完成
- //--------I2C重复起始信号-----------
- I2C1_CR2_START=1;//重复产生一个起始条件
- while(!(I2C1_SR1_SB==1));//读SR1寄存器,清除SB标志位
- //-------发送读I2C从器件地址---------
- I2C1_DR=0xD1;//发送从设备地址
- while(!(I2C1_SR1_ADDR==1));//读SR1寄存器,清除ADDR标志位
- if(I2C1_SR3_TRA==1)return 1;//读SR3寄存器,清除ADDR标志位
- //-------------读取数据-------------
- if(len>1)
- {
- for( i=len;i>1;i-- )
- {
- while(!(I2C1_SR1_RXNE==1));//等待I2C1_DR接收到数
- *rxbuf++ = I2C1_DR;
- }
- }
- //-------------停止信号-------------
- I2C1_CR2_ACK=0;//ACK位控制着ACK信号,此位为0可以产生一个NOACK信号
- I2C1_CR2_STOP=1;
- while(!(I2C1_SR1_RXNE==1));//等待I2C1_DR接收到数
- *rxbuf++ = I2C1_DR;
- return 0;
- }
- /******************************************************************************************************
- * 功 能:MPU6050初始化
- * 入口函数:
- * 出口函数:
- * 说 明:
- * 范 例:
- * 日 期:
- ******************************************************************************************************/
- void MPU6050_Config(void)
- {
- I2C_WriteOneByteDataToSlave(0x6B,0x02);//电源管理,唤醒MPU6050,时钟源选择:Y轴陀螺仪时钟倍频
- // I2C_WriteOneByteDataToSlave(0x6B,0x00);//电源管理,唤醒MPU6050,时钟源选择:Y轴陀螺仪时钟倍频
- I2C_WriteOneByteDataToSlave(0x1B,0x18);//陀螺仪最大量程 +-2000度每秒
- I2C_WriteOneByteDataToSlave(0x1C,0x1F);//加速度度最大量程 +-2G
- I2C_WriteOneByteDataToSlave(0x19,0x00);//陀螺仪采样率,不分频,8khz
- I2C_WriteOneByteDataToSlave(0x1A,0x00);//不启用低通滤波器
- // I2C_WriteOneByteDataToSlave(0x1A,0x01);//使用数字低通滤波器
- }
- /******************************************************************************************************
- * 功 能:该函数连续读取,MPU6050输出的加速度和角速度数据
- * 入口函数:a为存放读取到数据的数组名
- * 出口函数:
- * 说 明:在mpu6050.h头文件中宏定义了这个读取mpu6050数据的函数,一行不足写函数,所以进行了宏定义
- * 范 例:
- * 日 期:
- ******************************************************************************************************/
- #define Read_Data_form_MPU6050(a) I2C_ReadMultiBytesFromSlave(0x3B,a,14)//读取mpu6050输出的加速度和角速度数据
- /******************************************************************************************************
- * 功 能:
- * 入口函数:
- * 出口函数:
- * 说 明:将读取到的高8位和低8位数据合并成一个完整的16位数据
- * 范 例:
- * 日 期:
- ******************************************************************************************************/
- #define ACC_N 10
- #define GYRO_N 2
- void MPU6050_Data_Compose(uint8_t *buf)
- {
- static uint8_t acc_cnts=0,gyro_cnts=0;
- static int16_t Data_ACC[3][ACC_N]={0};
- static int16_t Data_GYRO[3][GYRO_N]={0};
- static int32_t sum_acc_x=0,sum_acc_y=0,sum_acc_z=0;
- static int32_t sum_gyro_x=0,sum_gyro_y=0,sum_gyro_z=0;
- //----合成加速度和角速度数据----
- mpu6050data_struct.accel_data[x]=((int16_t)buf[0]<<8) + buf[1];// - mpu6050data_struct.accel_offset_data[x] ;
- mpu6050data_struct.accel_data[y]=((int16_t)buf[2]<<8) + buf[3];// - mpu6050data_struct.accel_offset_data[y];
- mpu6050data_struct.accel_data[z]=((int16_t)buf[4]<<8) + buf[5];// - mpu6050data_struct.accel_offset_data[z];
- mpu6050data_struct.gyro_data[x] = ((int16_t)buf[8]<<8) + buf[9];// - mpu6050data_struct.gyro_offset_data[x];
- mpu6050data_struct.gyro_data[y] = ((int16_t)buf[10]<<8) + buf[11];// - mpu6050data_struct.gyro_offset_data[y];
- mpu6050data_struct.gyro_data[z] = ((int16_t)buf[12]<<8) + buf[13];// - mpu6050data_struct.gyro_offset_data[z];
- //----对加速度进行滑动窗口滤波----
- sum_acc_x-=Data_ACC[x][acc_cnts]; //减去最旧的一个数据
- sum_acc_y-=Data_ACC[y][acc_cnts];
- sum_acc_z-=Data_ACC[z][acc_cnts];
- Data_ACC[x][acc_cnts]=mpu6050data_struct.accel_data[x];//减去最旧的一个数据
- Data_ACC[y][acc_cnts]=mpu6050data_struct.accel_data[y];
- Data_ACC[z][acc_cnts]=mpu6050data_struct.accel_data[z];
- sum_acc_x+=Data_ACC[x][acc_cnts];
- sum_acc_y+=Data_ACC[y][acc_cnts];
- sum_acc_z+=Data_ACC[z][acc_cnts];
- acc_cnts++;
- if(acc_cnts==ACC_N)acc_cnts=0;
- mpu6050data_struct.accel_data[x]=sum_acc_x/ACC_N;//求平均值
- mpu6050data_struct.accel_data[y]=sum_acc_y/ACC_N;
- mpu6050data_struct.accel_data[z]=sum_acc_z/ACC_N;
- //----对角速度进行滑动窗口滤波----
- sum_gyro_x-=Data_GYRO[x][gyro_cnts];//减去最旧的一个数据
- sum_gyro_y-=Data_GYRO[y][gyro_cnts];
- sum_gyro_z-=Data_GYRO[z][gyro_cnts];
- Data_GYRO[x][gyro_cnts]=mpu6050data_struct.gyro_data[x];
- Data_GYRO[y][gyro_cnts]=mpu6050data_struct.gyro_data[y];
- Data_GYRO[z][gyro_cnts]=mpu6050data_struct.gyro_data[z];
- sum_gyro_x+=Data_GYRO[x][gyro_cnts];//加上最新的数据
- sum_gyro_y+=Data_GYRO[y][gyro_cnts];
- sum_gyro_z+=Data_GYRO[z][gyro_cnts];
- gyro_cnts++;
- if(gyro_cnts==GYRO_N)gyro_cnts=0;
- mpu6050data_struct.gyro_data[x]=sum_gyro_x/GYRO_N;//求平均值
- mpu6050data_struct.gyro_data[y]=sum_gyro_y/GYRO_N;
- mpu6050data_struct.gyro_data[z]=sum_gyro_z/GYRO_N;
- }
- /******************************************************************************************************
- * 功 能:
- * 入口函数:
- * 出口函数:
- * 说 明:
- * 范 例:
- * 日 期:
- ******************************************************************************************************/
- void Prepare_Data(void)
- {
- Read_Data_form_MPU6050(ui8Read_mpu6050_buffer);
- MPU6050_Data_Compose(ui8Read_mpu6050_buffer);
- }
- /******************************************************************************************************
- * 功 能:发送飞行姿态数据给匿名上位机
- * 入口函数:
- * 出口函数:
- * 说 明:根据匿名上位机协议发送数据
- * 范 例:
- * 日 期:
- ******************************************************************************************************/
- void SendDataToStatus(void)
- {
- int16_t _tmp;
- uint8_t sum = 0,i=0;
- ui8TX_buffer[0]=0xAA;
- ui8TX_buffer[1]=0xAA;
- ui8TX_buffer[2]=0x01;
- ui8TX_buffer[3]=12;
-
- _tmp = (int16_t)(Roll*100);//翻滚脚
- ui8TX_buffer[4] = (_tmp&0xFF00)>>8;//acc
- ui8TX_buffer[5] = _tmp&0x00FF;
-
- _tmp = (int16_t)(Pitch*100);//俯仰角
- ui8TX_buffer[6] = (_tmp&0xFF00)>>8;
- ui8TX_buffer[7] = _tmp&0x00FF;
-
- _tmp = (int16_t)(Yaw*100);//偏航角
- ui8TX_buffer[8] = (_tmp&0xFF00)>>8;
- ui8TX_buffer[9] = _tmp&0x00FF;
-
- ui8TX_buffer[10]=0;//没有超声波,只好发送无效数据
- ui8TX_buffer[11]=0;
- ui8TX_buffer[12]=0;
- ui8TX_buffer[13]=0;
- ui8TX_buffer[14]=0;//飞行模式
- ui8TX_buffer[15]=0;//解锁/加锁
-
- for(i=0;i<16;i++)
- sum += ui8TX_buffer[i];
-
- ui8TX_buffer[16] = sum;
- ui8TX_counter=17;
- PC_ODR_ODR7 ^=0x01;
- USART1_CR2_TEN=1;//打开发送
- USART1_CR2_TIEN=1;//打开发送中断
- }
- void main(void)
- {
- delay_100ms();
- CLK_CKDIVR =0x01;//8M
- GPIO_Init();
- UART_Init();
- I2C_Init();
- MPU6050_Config();
- asm("rim"); //enable interrupts
- while(1)
- {
- if(true==bTX_finished)
- {
- bTX_finished=false;
- Prepare_Data(); //读取MPU6050原始数据,并对数据进行简单滤波
- Pitch=atan((float)_ax/(float)_az)*57.3; //y轴固定,x轴方向与水平方向夹角
- Roll=atan((float)_ay/(float)_az)*57.3; //x轴固定,y轴方向与水平方向夹角
- SendDataToStatus();//发送姿态数据到上位机
- }
- // asm("wfi");
- }
- }
- /*************************************************************************************************** ***
- * 名 称: UART_OnTx()
- * 功 能: UART的Tx事件处理函数
- * 入口参数:无
- * 出口参数:无
- * 说 明:发送ui8TX_buffer数组中的数据,发送完成后,置位bTX_finished标志位,下一次进入中断后关闭发送
- * 范 例:无
- ******************************************************************************************************/
- #pragma vector=USART_T_TXE_vector
- __interrupt void USART_TX_ISR (void)
- {
- static uint8_t cnts=0;
- static bool bSend_EndByte=false;//发送最后一字节数据
- if(bSend_EndByte && USART1_SR_TC==1)
- {
- USART1_CR2_TIEN=0;//关闭发送中断
- USART1_CR2_TEN=0;//关闭发送
- bSend_EndByte=false;
- bTX_finished=true;
- }
- else if(USART1_SR_TC==1)
- {
- USART1_DR=ui8TX_buffer[cnts++];
- if(cnts==ui8TX_counter)
- {
- cnts=0;
- bSend_EndByte=true;
- }
- }
- }
- /******************************************************************************************************
- * 名 称: UART_OnRx()
- * 功 能: UART的Tx事件处理函数
- * 入口 参数:无
- * 出口 参数:无
- * 说 明:
- * 范 例:无
- ******************************************************************************************************/
- //#pragma vector=USART_R_RXNE_vector
- //__interrupt void USART_RX_ISR(void)
- //{
- // static uint8_t ui8RX_temp=0;
- // if(USART1_SR_RXNE==1)
- // {
- // ui8RX_temp = USART1_DR;
- // }
- //}
复制代码 |