www.99久,精品视频免费观看,一区二区三区视频

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

/* main.c file
功能：
1.初始化各個傳感器，
2.運行姿態(tài)解算和高度測量
3.將解算的姿態(tài)和各個傳感器的輸出上傳到 MiniIMU AHRS 測試軟件
4.響應(yīng) PC發(fā)送的命令
------------------------------------
*/
#include "common.h" //包含所有的驅(qū)動頭文件
//上傳數(shù)據(jù)的狀態(tài)機
#define REIMU 0x01 //上傳解算的姿態(tài)數(shù)據(jù)
#define REMOV 0x02 //上傳傳感器的輸出
#define REHMC 0x03 //上傳磁力計的標(biāo)定值
#define Upload_Speed 15 //數(shù)據(jù)上傳速度單位 Hz
#define upload_time (1000000/Upload_Speed)/2 //計算上傳的時間。單位為us
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t hx, hy, hz;
int32_t Temperature = 0, Pressure = 0, Altitude = 0;
uint32_t system_micrsecond;
int16_t hmcvalue[3];
u8 state= REIMU; //發(fā)送特定幀的狀態(tài)機
/**************************實現(xiàn)函數(shù)********************************************
*函數(shù)原型: int main(void)
*功　　能: 主程序
*******************************************************************************/
int main(void)
{
int16_t Math_hz=0;
unsigned char PC_comm; //PC 命令關(guān)鍵字節(jié)
float ypr[3]; // yaw pitch roll
/* 配置系統(tǒng)時鐘為72M 使用外部8M晶體+PLL*/
SystemInit();
delay_init(72); //延時初始化
Initial_LED_GPIO(); //初始化STM32-SDK板子上的LED接口
Initial_PWMLED();
Initial_UART1(115200L);
Initial_UART2(115200L);
IIC_Init(); //初始化I2C接口
delay_ms(300); //等待器件上電
//UART1_Put_String("Initialize...\r\n");
AHRS_init(); //初始化IMU和傳感器
system_micrsecond=micros();
while(1){ //主循環(huán)
//delay_ms(1); //延時，不要算那么快。
AHRS_getYawPitchRoll(ypr); //姿態(tài)更新
printf("%f %f %f\r\n",ypr[1],ypr[2],ypr[0]);
Math_hz++; //解算次數(shù) ++
BMP180_Routing(); //處理BMP018 事務(wù) 開啟轉(zhuǎn)換和讀取結(jié)果將在這個子程序中進行
//-------------上位機------------------------------
//是否到了更新上位機的時間了？
if((micros()-system_micrsecond)>upload_time){
switch(state){
case REIMU:
BMP180_getTemperat(&Temperature); //讀取最近的溫度值
BMP180_getPress(&Pressure); //讀取最近的氣壓測量值
BMP180_getAlt(&Altitude); //讀取相對高度
//UART1_ReportIMU((int16_t)(ypr[0]*10.0),(int16_t)(ypr[1]*10.0),
//(int16_t)(ypr[2]*10.0),Altitude/10,Temperature,Pressure/10,Math_hz*16);
//UART2_ReportIMU((int16_t)(ypr[0]*10.0),(int16_t)(ypr[1]*10.0),
//(int16_t)(ypr[2]*10.0),Altitude/10,Temperature,Pressure/10,Math_hz*Upload_Speed);
Math_hz=0;
state = REMOV; //更改狀態(tài)。
break;
case REMOV:
MPU6050_getlastMotion6(&ax, &ay, &az, &gx, &gy, &gz);
HMC58X3_getlastValues(&hx,&hy,&hz);
//UART1_ReportMotion(ax,ay,az,gx,gy,gz,hx,hy,hz);
//UART2_ReportMotion(ax,ay,az,gx,gy,gz,hx,hy,hz);
state = REIMU;
break;
default:
state = REIMU;
break;
}//switch(state)
system_micrsecond=micros(); //取系統(tǒng)時間單位 us
LED_Change(); //LED1改變亮度
}
//--------------------------------------------------
//處理PC發(fā)送來的命令
if((PC_comm=UART2_CommandRoute())!=0xff)
{
switch(PC_comm){ //檢查命令標(biāo)識
case Gyro_init: MPU6050_InitGyro_Offset(); break; //讀取陀螺儀零偏
case High_init: BMP180_ResetAlt(0); break; //氣壓高度清零
}
}// 處理PC 發(fā)送的命令
}//主循環(huán) while(1) 結(jié)束
} //main
//------------------End of File----------------------------

復(fù)制代碼