|
|
基于STM32F103ZE+mpu6050+hmc5883L通過(guò)卡爾曼濾波串口輸出姿態(tài)角，數(shù)據(jù)經(jīng)過(guò)融合，Z無(wú)漂移，絕對(duì)指向
單片機(jī)源程序如下:
- /* main.c file
- 功能：
- 1.初始化各個(gè)傳感器，
- 2.運(yùn)行姿態(tài)解算和高度測(cè)量
- 3.將解算的姿態(tài)和各個(gè)傳感器的輸出上傳到 MiniIMU AHRS 測(cè)試軟件
- 4.響應(yīng) PC發(fā)送的命令
- ------------------------------------
- */
- #include "common.h" //包含所有的驅(qū)動(dòng) 頭文件
- //上傳數(shù)據(jù)的狀態(tài)機(jī)
- #define REIMU 0x01 //上傳解算的姿態(tài)數(shù)據(jù)
- #define REMOV 0x02 //上傳傳感器的輸出
- #define REHMC 0x03 //上傳磁力計(jì)的標(biāo)定值
- #define Upload_Speed 15 //數(shù)據(jù)上傳速度 單位 Hz
- #define upload_time (1000000/Upload_Speed)/2 //計(jì)算上傳的時(shí)間。單位為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)機(jī)
- /**************************實(shí)現(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)時(shí)鐘為72M 使用外部8M晶體+PLL*/
- SystemInit();
- delay_init(72); //延時(shí)初始化
- 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); //延時(shí)，不要算那么快。
- AHRS_getYawPitchRoll(ypr); //姿態(tài)更新
- printf("%f %f %f\r\n",ypr[1],ypr[2],ypr[0]);
- Math_hz++; //解算次數(shù) ++
- BMP180_Routing(); //處理BMP018 事務(wù) 開(kāi)啟轉(zhuǎn)換和讀取結(jié)果將在這個(gè)子程序中進(jìn)行
- //-------------上位機(jī)------------------------------
- //是否到了更新 上位機(jī)的時(shí)間了？
- if((micros()-system_micrsecond)>upload_time){
- switch(state){
- case REIMU:
- BMP180_getTemperat(&Temperature); //讀取最近的溫度值
- BMP180_getPress(&Pressure); //讀取最近的氣壓測(cè)量值
- BMP180_getAlt(&Altitude); //讀取相對(duì)高度
- //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)時(shí)間 單位 us
- LED_Change(); //LED1改變亮度
- }
- //--------------------------------------------------
- //處理PC發(fā)送來(lái)的命令
- if((PC_comm=UART2_CommandRoute())!=0xff)
- {
- switch(PC_comm){ //檢查命令標(biāo)識(shí)
- 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----------------------------
所有資料51hei提供下載:
Kalman_AHRS.7z
(308.96 KB, 下載次數(shù): 298)
2019-7-10 19:14 上傳
點(diǎn)擊文件名下載附件
|
評(píng)分
-
查看全部評(píng)分
|
[復(fù)制鏈接]
