基于STM8L的MPU6050數(shù)據(jù)分析(上)

ID:140343 · 發(fā)表于 2016-10-9 23:48

MPU6050包含了加速度和角速度傳感器,通過(guò)STM8L的I2C可以讀取到這些加速度和角速度數(shù)據(jù).
讀取到的數(shù)據(jù)是MPU6050的內(nèi)部AD在加速度和角速度的X,Y,Z軸的AD采樣值.根據(jù) RX = ADCRx采樣值/靈敏度 ,可以求出各軸的實(shí)際物理值.

對(duì)于靈敏度,拿加速度計(jì)來(lái)說(shuō),如果設(shè)置MPU6050的加速度量程是+-2G,為了表示+2G~-2G這樣一個(gè)范圍,規(guī)定16位的寄存器最高位是符號(hào)位,表示正負(fù),剩下的15位表示數(shù)據(jù).因?yàn)?^15=32768,所以當(dāng)寄存器中的數(shù)據(jù)為32768時(shí),表示+2G,同樣-32768表示-2G.1G應(yīng)該是16384,所以+-2G的量程下,靈敏度為16384.
例如,當(dāng)加速度的X軸讀取到的數(shù)據(jù)是-16384,因?yàn)?16384/16384=-1,所以此時(shí)x軸的加速度為-1G.
如何根據(jù)加速度計(jì)各個(gè)軸的輸出計(jì)算此時(shí)的角度呢?

如上圖,加速度的y軸不動(dòng),x軸繞其旋轉(zhuǎn)一定的角度a,這個(gè)角度是x軸與水平面的夾角.
上圖中的g是重力加速度,當(dāng)MPU6050按照上圖傾斜一定角度a時(shí),此時(shí)重力加速度在gz和gx方向上各有一個(gè)分力.根據(jù)物理中的力的分解合成,gx和gz上的分力合合成重力.
因?yàn)橹亓方向與水平面垂直,所以a+b=90°
因?yàn)間z軸和gx軸垂直,所以c+b=90°
綜上c=a
根據(jù)重力合成,c和d是相等的.所以角a和角d是相等的.
tan(d)=gx/gz,即tan(a)=gx/gz
為了求角a,我們可以根據(jù)mpu6050的輸出數(shù)據(jù)得到gx/gz的比值,再根據(jù)數(shù)學(xué)公式中的反正切,就可以求出此時(shí)的角度值.
當(dāng)加速度的x軸不動(dòng),y軸繞其旋轉(zhuǎn)一定的角度a時(shí),通過(guò)同樣的方法也可以求出此時(shí)的角度.
通過(guò)上面的分析,我們可以通過(guò)mpu6050或其他一些具有加速度計(jì)的傳感器,獲取物體在靜止時(shí),在x軸方向和y軸方向與水平面的夾角.
之所以說(shuō)是靜止,是因?yàn)檫\(yùn)動(dòng)時(shí)會(huì)有運(yùn)動(dòng)加速度,這對(duì)角度的測(cè)量會(huì)產(chǎn)生干擾,所以角速度傳感器測(cè)量角度會(huì)存在易受運(yùn)動(dòng)影響的干擾.

角速度傳感器同樣是為了獲得角度,角速度是單位時(shí)間內(nèi)旋轉(zhuǎn)過(guò)的角度,注定這是一個(gè)動(dòng)態(tài)的物理量,物體靜止時(shí),角速度為0.如上圖,物體繞x軸方向旋轉(zhuǎn)一定角度,我們把旋轉(zhuǎn)這個(gè)動(dòng)態(tài)過(guò)程分解,假設(shè)第1秒時(shí),旋轉(zhuǎn)到1位置,第2秒時(shí),旋轉(zhuǎn)到2位置,第2秒時(shí),旋轉(zhuǎn)到3位置.根據(jù)角速度,可以求出第1秒旋轉(zhuǎn)過(guò)的角度,同樣求出第2秒,第3秒的角度,將這些角度想加,就可以得到3秒的時(shí)間旋轉(zhuǎn)過(guò)的角度.
本例程的總體思路是,通過(guò)STM8L讀取MPU6050的加速度和角速度數(shù)據(jù),根據(jù)加速度值,求出x方向和y方向上的角度變化,僅僅是使用了上文中提及的反正切函數(shù).然后用匿名上位機(jī),動(dòng)態(tài)展示計(jì)算后的角度.

本例程中,對(duì)加速度和角速度數(shù)據(jù),進(jìn)行了簡(jiǎn)單的滑動(dòng)窗口濾波.STM8L的主頻為8MHz,I2C的通信速率為400KHz,USART通信速率為115200.
本例程的視頻演示,由于只使用了加速度可以看到,角度輸出并不是什么平穩(wěn),受到運(yùn)動(dòng)加速度的干擾.

/*硬件連接*/
// PC0<--->SDA PC1---->SCL
//USART1_TX on PC3 and USART1_RX on PC2
/****************************************************************************************
*開(kāi)發(fā)環(huán)境:IAR for stm8 v6.5.3
*硬件平臺(tái):STM8L-DISCOVERY
*功能說(shuō)明:通過(guò)硬件I2C等待的方法,
*作者：茗風(fēng)
****************************************************************************************/
#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原始加速度計(jì)值
// int16_t temp;//溫度數(shù)據(jù)
int16_t gyro_offset_data[3];//水平靜止時(shí)陀螺儀偏移值
int16_t accel_offset_data[3];//水平靜止時(shí)加速度計(jì)偏移值
};
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};
/******************************************************************************************************
* 名稱(chēng)：void delay_10ms(uint8_t x_ms)
* 功能：延時(shí)10ms
* 入口參數(shù)：無(wú)
* 出口參數(shù)：無(wú)
* 說(shuō) 明：
* 范例：無(wú)
******************************************************************************************************/
void delay_100ms(void)
{
uint8_t i,j;
for(i=0;i<255;i++)//2*255個(gè)指令周期
for(j=0;j<255;j++);//2*255個(gè)指令周期
// delay_10ms共消耗 x_ms*2*255+2*x_ms個(gè)指令周期
// 255*2*255+2*255=130610us=130ms
// 此延時(shí)函數(shù)，延時(shí)時(shí)間為130ms
// 16M/8/2=1M 一個(gè)指令周期為1us
}
/******************************************************************************************************
* 名稱(chēng)：void GPIO_Init(void)
* 功能：初始化PC7為高速推挽輸出
* 入口參數(shù)：無(wú)
* 出口參數(shù)：無(wú)
* 說(shuō) 明：
* 范例：無(wú)
******************************************************************************************************/
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;//輸出低電平
}
/******************************************************************************************************
* 名稱(chēng)：void UART_Init(void)
* 功能：UART定時(shí)器初始化系統(tǒng)
* 入口參數(shù)：無(wú)
* 出口參數(shù)：無(wú)
* 說(shuō) 明：UART初始化為 115200 1 8 n
* 范例：無(wú)
******************************************************************************************************/
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空閑狀態(tài)為高電平，如果不設(shè)置，會(huì)莫名奇妙的發(fā)送0x00
CLK_PCKENR1_PCKEN15=1;//開(kāi)啟USART外設(shè)時(shí)鐘
//開(kāi)啟引腳的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
//如果需要使用其他引腳作為串口發(fā)送接收引腳，請(qǐng)?jiān)谶@里選擇
//設(shè)置串口工作方式
USART1_CR1_M =0;//1 start bit，8 data bit，n stop bit
USART1_CR3_STOP0=0;USART1_CR3_STOP1=0;//1 stop bit
//設(shè)置波特率
//波特率設(shè)置為115200
// 8000000/115200=69 //8000000是此時(shí)系統(tǒng)的主時(shí)鐘頻率，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;//使能發(fā)送
// USART1_CR2_TIEN=0;//打開(kāi)發(fā)送中斷
// USART1_CR2_REN=1;//使能接收
// USART1_CR2_RIEN=1;//打開(kāi)接收中斷
}
/******************************************************************************************************
* 名稱(chēng)： IIC_init()
* 功能：初始化I2C,系統(tǒng)主頻位4MHz，I2C通信速度位333KHz
* 入口參數(shù)：無(wú)
* 出口參數(shù)：無(wú)
* 說(shuō) 明：PC0--SDA PC1--SCL
* 范例：無(wú)
******************************************************************************************************/
void I2C_Init(void)
{
//----打開(kāi)IIC外設(shè)時(shí)鐘----
CLK_PCKENR1_PCKEN13=1;//
I2C1_CR1_PE=0;
I2C1_CR2_ACK=1;
//----I2C輸入時(shí)鐘頻率選擇----
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 */
//----配置時(shí)鐘控制寄存器----
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高電平時(shí)間配置
//I2C的SCK時(shí)鐘設(shè)置為400KHz,則SCK周期為2.5us 2.5us/0.125/3=7
//因?yàn)镮2C1_FREQR_FREQ=0x08,即I2C輸入時(shí)鐘頻率為8M,周期為0.125us
//CCR=7時(shí),SCK的低電平時(shí)間為2*tlow=2*7*0.125us=1.75us,SCk高電平時(shí)間為thigh=7*0.125us=0.875us
//所以CCR=7時(shí),SCK輸出頻率為380KHz
//----配置上升時(shí)間寄存器----
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;//
}
/******************************************************************************************************
* 名稱(chēng)： uint8_t I2C_ReadOneByteDataFromSlave(uint8_t address)
* 功能：從I2C從設(shè)備中讀取一字節(jié)的數(shù)據(jù)
* 入口參數(shù)：address:讀取數(shù)據(jù)的寄存器地址
* 出口參數(shù)：返回一個(gè)從I2C從設(shè)備指定地址讀到的數(shù)據(jù)
* 說(shuō) 明：
* 范例：無(wú)
******************************************************************************************************/
uint8_t I2C_ReadOneByteDataFromSlave(uint8_t address)
{
volatile uint8_t t;
//----------I2C起始信號(hào)--------------
I2C1_CR2_START=1;//產(chǎn)生一個(gè)起始條件
while(!(I2C1_SR1_SB==1));//讀SR1寄存器，清除SB標(biāo)志位
// _5NOPS;//根據(jù)數(shù)據(jù)手冊(cè),檢測(cè)到標(biāo)志位后,需插入5個(gè)NOP進(jìn)行延時(shí)
//-------發(fā)送寫(xiě)I2C從器件地址---------
I2C1_DR=0xD0;//發(fā)送從設(shè)備地址
while(!(I2C1_SR1_ADDR==1));//讀SR1寄存器，清除ADDR標(biāo)志位
// _5NOPS;//根據(jù)數(shù)據(jù)手冊(cè),檢測(cè)到標(biāo)志位后,需插入5個(gè)NOP進(jìn)行延時(shí)
if(I2C1_SR3_TRA==0)return 1;//讀SR3寄存器，清除ADDR標(biāo)志位
// 0: Data bytes received
// 1: Data bytes transmitted
//-----寫(xiě)I2C從器件寄存器地址--------
I2C1_DR=address;
while(!(I2C1_SR1_BTF==1));//等待地址發(fā)送完成
// _5NOPS;//根據(jù)數(shù)據(jù)手冊(cè),檢測(cè)到標(biāo)志位后,需插入5個(gè)NOP進(jìn)行延時(shí)
//--------I2C重復(fù)起始信號(hào)-----------
I2C1_CR2_START=1;//重復(fù)產(chǎn)生一個(gè)起始條件
while(!(I2C1_SR1_SB==1));//讀SR1寄存器，清除SB標(biāo)志位
// _5NOPS;//根據(jù)數(shù)據(jù)手冊(cè),檢測(cè)到標(biāo)志位后,需插入5個(gè)NOP進(jìn)行延時(shí)
//-------發(fā)送讀I2C從器件地址---------
I2C1_DR=0xD1;//發(fā)送從設(shè)備地址
while(!(I2C1_SR1_ADDR==1));//讀SR1寄存器，清除ADDR標(biāo)志位
// _5NOPS;//根據(jù)數(shù)據(jù)手冊(cè),檢測(cè)到標(biāo)志位后,需插入5個(gè)NOP進(jìn)行延時(shí)
if(I2C1_SR3_TRA==1)return 1;//讀SR3寄存器，清除ADDR標(biāo)志位
//-------------停止信號(hào)-------------
I2C1_CR2_ACK=0;//ACK位控制著ACK信號(hào),此位為0可以產(chǎn)生一個(gè)NOACK信號(hào)
I2C1_CR2_STOP=1;
//-------------等待接收到數(shù)據(jù)-------------
while(!(I2C1_SR1_RXNE==1));//等待地址發(fā)送完成
//-------------讀取數(shù)據(jù)-------------
t=I2C1_DR;
return t;
}
/******************************************************************************************************
* 名稱(chēng)：void I2C_WriteOneByteDataToSlave(uint8_t address,uint8_t dat)
* 功能：寫(xiě)入一字節(jié)的數(shù)據(jù)到I2C設(shè)備中
* 入口參數(shù)：address:寫(xiě)入的數(shù)據(jù)存儲(chǔ)地址 dat:待寫(xiě)入的數(shù)據(jù)
* 出口參數(shù)：無(wú)
* 說(shuō) 明：通過(guò)MSTM8L硬件寫(xiě)入I2C設(shè)備一個(gè)字節(jié)的數(shù)據(jù)
* 范例：無(wú)
******************************************************************************************************/
uint8_t I2C_WriteOneByteDataToSlave(uint8_t address,uint8_t dat)
{
volatile uint8_t t;
I2C1_CR2_ACK=1;
//----------I2C起始信號(hào)--------------
I2C1_CR2_START=1;//產(chǎn)生一個(gè)起始條件
while(!(I2C1_SR1_SB==1));
// _5NOPS;//根據(jù)數(shù)據(jù)手冊(cè),檢測(cè)到標(biāo)志位后,需插入5個(gè)NOP進(jìn)行延時(shí)
I2C1_DR=0xD0;
//--------寫(xiě)I2C從器件地址-----------
while(!(I2C1_SR1_ADDR==1));
// _5NOPS;//根據(jù)數(shù)據(jù)手冊(cè),檢測(cè)到標(biāo)志位后,需插入5個(gè)NOP進(jìn)行延時(shí)
if(I2C1_SR3_TRA==0)return 1;//讀SR3寄存器，清除ADDR標(biāo)志位
//-----寫(xiě)I2C從器件寄存器地址--------
while(!(I2C1_SR1_TXE==1));
I2C1_DR=address;
//-------寫(xiě)I2C數(shù)據(jù)到寄存器中--------
while(!(I2C1_SR1_TXE==1));
I2C1_DR=dat;
while(!(I2C1_SR1_TXE==1));
while(!(I2C1_SR1_BTF==1));
// _5NOPS;//根據(jù)數(shù)據(jù)手冊(cè),檢測(cè)到標(biāo)志位后,需插入5個(gè)NOP進(jìn)行延時(shí)
//-------------停止信號(hào)-------------
I2C1_CR2_STOP=1;
return 0;
}
/******************************************************************************************************
* 功能：從I2C從設(shè)備讀取多個(gè)字節(jié)數(shù)據(jù)
* 入口函數(shù):
* 出口函數(shù)：
* 說(shuō) 明：
* 范例：
* 日期：
******************************************************************************************************/
uint8_t I2C_ReadMultiBytesFromSlave(uint8_t address,uint8_t *rxbuf,uint8_t len)
{
volatile uint8_t i=0;
if(len==0)return 1;//如果寫(xiě)入字節(jié)長(zhǎng)度為0退出
I2C1_CR2_ACK=1;
//----------I2C起始信號(hào)--------------
I2C1_CR2_START=1;//產(chǎn)生一個(gè)起始條件
while(!(I2C1_SR1_SB==1));//讀SR1寄存器，清除SB標(biāo)志位
//-------發(fā)送寫(xiě)I2C從器件地址---------
I2C1_DR=0xD0;//發(fā)送從設(shè)備地址
while(!(I2C1_SR1_ADDR==1));//讀SR1寄存器，清除ADDR標(biāo)志位
if(I2C1_SR3_TRA==0)return 1;//讀SR3寄存器，清除ADDR標(biāo)志位
// 0: Data bytes received
// 1: Data bytes transmitted
//-----寫(xiě)I2C從器件寄存器地址--------
I2C1_DR=address;
while(!(I2C1_SR1_BTF==1));//等待地址發(fā)送完成
//--------I2C重復(fù)起始信號(hào)-----------
I2C1_CR2_START=1;//重復(fù)產(chǎn)生一個(gè)起始條件
while(!(I2C1_SR1_SB==1));//讀SR1寄存器，清除SB標(biāo)志位
//-------發(fā)送讀I2C從器件地址---------
I2C1_DR=0xD1;//發(fā)送從設(shè)備地址
while(!(I2C1_SR1_ADDR==1));//讀SR1寄存器，清除ADDR標(biāo)志位
if(I2C1_SR3_TRA==1)return 1;//讀SR3寄存器，清除ADDR標(biāo)志位
//-------------讀取數(shù)據(jù)-------------
if(len>1)
{
for( i=len;i>1;i-- )
{
while(!(I2C1_SR1_RXNE==1));//等待I2C1_DR接收到數(shù)
*rxbuf++ = I2C1_DR;
}
}
//-------------停止信號(hào)-------------
I2C1_CR2_ACK=0;//ACK位控制著ACK信號(hào),此位為0可以產(chǎn)生一個(gè)NOACK信號(hào)
I2C1_CR2_STOP=1;
while(!(I2C1_SR1_RXNE==1));//等待I2C1_DR接收到數(shù)
*rxbuf++ = I2C1_DR;
return 0;
}
/******************************************************************************************************
* 功能：MPU6050初始化
* 入口函數(shù):
* 出口函數(shù)：
* 說(shuō) 明：
* 范例：
* 日期：
******************************************************************************************************/
void MPU6050_Config(void)
{
I2C_WriteOneByteDataToSlave(0x6B,0x02);//電源管理,喚醒MPU6050,時(shí)鐘源選擇:Y軸陀螺儀時(shí)鐘倍頻
// I2C_WriteOneByteDataToSlave(0x6B,0x00);//電源管理,喚醒MPU6050,時(shí)鐘源選擇:Y軸陀螺儀時(shí)鐘倍頻
I2C_WriteOneByteDataToSlave(0x1B,0x18);//陀螺儀最大量程 +-2000度每秒
I2C_WriteOneByteDataToSlave(0x1C,0x1F);//加速度度最大量程 +-2G
I2C_WriteOneByteDataToSlave(0x19,0x00);//陀螺儀采樣率，不分頻，8khz
I2C_WriteOneByteDataToSlave(0x1A,0x00);//不啟用低通濾波器
// I2C_WriteOneByteDataToSlave(0x1A,0x01);//使用數(shù)字低通濾波器
}
/******************************************************************************************************
* 功能：該函數(shù)連續(xù)讀取,MPU6050輸出的加速度和角速度數(shù)據(jù)
* 入口函數(shù):a為存放讀取到數(shù)據(jù)的數(shù)組名
* 出口函數(shù)：
* 說(shuō) 明：在mpu6050.h頭文件中宏定義了這個(gè)讀取mpu6050數(shù)據(jù)的函數(shù)，一行不足寫(xiě)函數(shù)，所以進(jìn)行了宏定義
* 范例：
* 日期：
******************************************************************************************************/
#define Read_Data_form_MPU6050(a) I2C_ReadMultiBytesFromSlave(0x3B,a,14)//讀取mpu6050輸出的加速度和角速度數(shù)據(jù)
/******************************************************************************************************
* 功能：
* 入口函數(shù):
* 出口函數(shù)：
* 說(shuō) 明:將讀取到的高8位和低8位數(shù)據(jù)合并成一個(gè)完整的16位數(shù)據(jù)
* 范例：
* 日期：
******************************************************************************************************/
#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;
//----合成加速度和角速度數(shù)據(jù)----
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];
//----對(duì)加速度進(jìn)行滑動(dòng)窗口濾波----
sum_acc_x-=Data_ACC[x][acc_cnts]; //減去最舊的一個(gè)數(shù)據(jù)
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];//減去最舊的一個(gè)數(shù)據(jù)
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;
//----對(duì)角速度進(jìn)行滑動(dòng)窗口濾波----
sum_gyro_x-=Data_GYRO[x][gyro_cnts];//減去最舊的一個(gè)數(shù)據(jù)
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];//加上最新的數(shù)據(jù)
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;
}
/******************************************************************************************************
* 功能：
* 入口函數(shù):
* 出口函數(shù)：
* 說(shuō) 明：
* 范例：
* 日期：
******************************************************************************************************/
void Prepare_Data(void)
{
Read_Data_form_MPU6050(ui8Read_mpu6050_buffer);
MPU6050_Data_Compose(ui8Read_mpu6050_buffer);
}
/******************************************************************************************************
* 功能：發(fā)送飛行姿態(tài)數(shù)據(jù)給匿名上位機(jī)
* 入口函數(shù):
* 出口函數(shù)：
* 說(shuō) 明：根據(jù)匿名上位機(jī)協(xié)議發(fā)送數(shù)據(jù)
* 范例：
* 日期：
******************************************************************************************************/
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;//沒(méi)有超聲波，只好發(fā)送無(wú)效數(shù)據(jù)
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;//打開(kāi)發(fā)送
USART1_CR2_TIEN=1;//打開(kāi)發(fā)送中斷
}
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原始數(shù)據(jù),并對(duì)數(shù)據(jù)進(jìn)行簡(jiǎn)單濾波
Pitch=atan((float)_ax/(float)_az)*57.3; //y軸固定,x軸方向與水平方向夾角
Roll=atan((float)_ay/(float)_az)*57.3; //x軸固定,y軸方向與水平方向夾角
SendDataToStatus();//發(fā)送姿態(tài)數(shù)據(jù)到上位機(jī)
}
// asm("wfi");
}
}
/*************************************************************************************************** ***
* 名稱(chēng)： UART_OnTx()
* 功能： UART的Tx事件處理函數(shù)
* 入口參數(shù)：無(wú)
* 出口參數(shù)：無(wú)
* 說(shuō) 明：發(fā)送ui8TX_buffer數(shù)組中的數(shù)據(jù)，發(fā)送完成后，置位bTX_finished標(biāo)志位，下一次進(jìn)入中斷后關(guān)閉發(fā)送
* 范例：無(wú)
******************************************************************************************************/
#pragma vector=USART_T_TXE_vector
__interrupt void USART_TX_ISR (void)
{
static uint8_t cnts=0;
static bool bSend_EndByte=false;//發(fā)送最后一字節(jié)數(shù)據(jù)
if(bSend_EndByte && USART1_SR_TC==1)
{
USART1_CR2_TIEN=0;//關(guān)閉發(fā)送中斷
USART1_CR2_TEN=0;//關(guān)閉發(fā)送
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;
}
}
}
/******************************************************************************************************
* 名稱(chēng)： UART_OnRx()
* 功能： UART的Tx事件處理函數(shù)
* 入口參數(shù)：無(wú)
* 出口參數(shù)：無(wú)
* 說(shuō) 明：
* 范例：無(wú)
******************************************************************************************************/
//#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;
// }
//}

復(fù)制代碼

ID:221145 · 發(fā)表于 2017-10-16 17:36

樓主牛逼，下在那里，沒(méi)看到啊~~·

帳號(hào)		自動(dòng)登錄	找回密碼
密碼			立即注冊(cè)

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

基于STM8L的MPU6050數(shù)據(jù)分析(上)