国产精品一区二区在线观看,日韩中文字幕在线播放,中文字幕视频一区

/*硬件連接*/
// 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;
// }
//}

復制代碼

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