|
|
|
臨近期末，學(xué)校剛剛考完試，弄了一個(gè)基于STM32F429的姿態(tài)檢測(cè)系統(tǒng)，也就是俗稱(chēng)的AHRS，被很多人用來(lái)做四軸飛行器的，不過(guò)本人對(duì)四軸飛行器不是很感興趣，主要用來(lái)檢測(cè)人體姿態(tài)和仿生控制的。采用MPU6050進(jìn)行數(shù)據(jù)融合，得到空間姿態(tài)信息，從而完成手勢(shì)檢測(cè)，動(dòng)作檢測(cè)，人體姿態(tài)檢測(cè)�；�MEMS的三軸陀螺儀，加速度計(jì)。目前AHRS內(nèi)部采用的多傳感器數(shù)據(jù)融合進(jìn)行的航姿解算單元為卡爾曼濾波器。
特點(diǎn)：高精度360 度全方位位置姿態(tài)輸出，數(shù)據(jù)融合算法快速動(dòng)態(tài)響應(yīng)與長(zhǎng)時(shí)間穩(wěn)定性（無(wú)漂移，無(wú)積累誤差）
輸出：三維全姿態(tài)數(shù)據(jù)（四元數(shù) / 歐拉角 / 旋轉(zhuǎn)矩陣/原始數(shù)據(jù)）三維加速度 / 三維角速度
不多說(shuō)了上圖！相關(guān)原理請(qǐng)自己谷歌一下
- /*-----------------------------------------------------------------------------------
- STM32F429ZI+MPU6050+AHRS
-
- 簡(jiǎn)介:
-
- 1) 如何計(jì)算積分
- 對(duì)角速度積分得到角度,但是會(huì)累積誤差,dt為積分時(shí)間s秒,最好放在中斷運(yùn)行.中斷每隔
- dt秒進(jìn)入一次中斷并進(jìn)行計(jì)算,必須確保在1個(gè)中斷周期內(nèi)能夠完成全部計(jì)算.
-
- Example:
- Axis.GyroY = getGyroY(); //獲取Y軸角速度,單位為度
- Angle += Axis.GyroY * dt; //離散信號(hào)的積分即為求黎曼和.
-
- 2) 互補(bǔ)濾波器
- 原理是取當(dāng)前傾角和加速度,做差獲得傾角差值并進(jìn)行放大,然后與陀螺儀角速度
- 疊加后再積分,從而使傾角跟蹤為加速度獲得的角度.
- 2為放大倍數(shù),可調(diào)節(jié)補(bǔ)償度;0.01為系統(tǒng)周期10ms.
-
- Example:
- FilterAngle = FilterAngle + (((Gesture.Angle_FY - FilterAngle)*2 + Axis.GyroY)*0.01);
-
- 3) 卡爾曼濾波器
- 原理請(qǐng)百度
-
- Example:
- Kalman_Filter(Gesture.Angle_FY,Axis.GyroY,&FilterAngle);
-
- 4) AHRS數(shù)據(jù)融合
- 通過(guò)四元數(shù)矩陣運(yùn)算把加速度計(jì)和陀螺儀的數(shù)據(jù)進(jìn)行融合計(jì)算出歐拉角.
-
-
- +------------------------+-----------------------+----------------------------+
- + LCD GPIO分配表 +
- +------------------------+-----------------------+----------------------------+
- | LCD_TFT R2 <-> PC10 | LCD_TFT G2 <-> PA6 | LCD_TFT B2 <-> PD6 |
- | LCD_TFT R3 <-> PB0 | LCD_TFT G3 <-> PG10 | LCD_TFT B3 <-> PG11 |
- | LCD_TFT R4 <-> PA11 | LCD_TFT G4 <-> PB10 | LCD_TFT B4 <-> PG12 |
- | LCD_TFT R5 <-> PA12 | LCD_TFT G5 <-> PB11 | LCD_TFT B5 <-> PA3 |
- | LCD_TFT R6 <-> PB1 | LCD_TFT G6 <-> PC7 | LCD_TFT B6 <-> PB8 |
- | LCD_TFT R7 <-> PG6 | LCD_TFT G7 <-> PD3 | LCD_TFT B7 <-> PB9 |
- -------------------------------------------------------------------------------
- | LCD_TFT HSYNC <-> PC6 | LCDTFT VSYNC <-> PA4 |
- | LCD_TFT CLK <-> PG7 | LCD_TFT DE <-> PF10 |
- -----------------------------------------------------
- +-------------------+--------------------+--------------------+--------------------+
- + SDRAM GPIO 分配表 +
- +-------------------+--------------------+--------------------+--------------------+
- | PD0 <-> FMC_D2 | PE0 <-> FMC_NBL0 | PF0 <-> FMC_A0 | PG0 <-> FMC_A10 |
- | PD1 <-> FMC_D3 | PE1 <-> FMC_NBL1 | PF1 <-> FMC_A1 | PG1 <-> FMC_A11 |
- | PD8 <-> FMC_D13 | PE7 <-> FMC_D4 | PF2 <-> FMC_A2 | PG8 <-> FMC_SDCLK |
- | PD9 <-> FMC_D14 | PE8 <-> FMC_D5 | PF3 <-> FMC_A3 | PG15 <-> FMC_NCAS |
- | PD10 <-> FMC_D15 | PE9 <-> FMC_D6 | PF4 <-> FMC_A4 |--------------------+
- | PD14 <-> FMC_D0 | PE10 <-> FMC_D7 | PF5 <-> FMC_A5 |
- | PD15 <-> FMC_D1 | PE11 <-> FMC_D8 | PF11 <-> FMC_NRAS |
- +-------------------| PE12 <-> FMC_D9 | PF12 <-> FMC_A6 |
- | PE13 <-> FMC_D10 | PF13 <-> FMC_A7 |
- | PE14 <-> FMC_D11 | PF14 <-> FMC_A8 |
- | PE15 <-> FMC_D12 | PF15 <-> FMC_A9 |
- +-------------------+--------------------+--------------------+
- | PB5 <-> FMC_SDCKE1|
- | PB6 <-> FMC_SDNE1 |
- | PC0 <-> FMC_SDNWE |
- +-------------------+
-
- 作者: BoX
- 版本: V1.0
- 平臺(tái): STM32F429I-Discovery Board
- 日期: 2014年7月11日
- ------------------------------------------------------------------------------------*/
- #include "LED.h"
- #include "stm32f4_tft.h"
- #include "stm32f4_delay.h"
- #include "stm32f4_usart1.h"
- #include "stm32f4_timer.h"
- #include "stm32f4_ahrs.h"
- #include "stm32f4_mpu6050.h"
- #include "iic_analog.h"
- #include "kalman_filter.h"
- #include "stm32f4xx.h"
- /*------------------------------------------
- 條件編譯
- DATA_TYPE
- 0 - 輸出處理后的數(shù)據(jù),加速度單位為g,角速度
- 單位為deg/s
- 1 - 輸出數(shù)據(jù)融合后的歐拉角.
- 2 - 輸出卡爾曼濾波后的角度.
-
- DATA_DISPLAY
- 0 - 數(shù)據(jù)不顯示在LCD上
- 1 - 數(shù)據(jù)顯示在LCD上
- ------------------------------------------*/
- #define USAR_DELAY (800)
- #define DATA_TYPE (1)
- #define DATA_DISPLAY (1)
- /*------------------------------------------
- 全局變量
- ------------------------------------------*/
- extern MPU6050_AxisTypeDef Axis;
- extern AHRS_EulerAngleTypeDef EulerAngle;
- uint8_t LCD_Buffer[240];
- /*------------------------------------------
- 主函數(shù)
- ------------------------------------------*/
- int main(void)
- {
- DelayInit();
- LED_Init();
- USART1_Config();
- printf(" AHRS OK!");
- MPU6050_Init();
- TIM2_Config(2000,89); //每2ms溢出中斷為AHRS_Update提供定時(shí)
- LCD_Init();
- LCD_LayerInit();
- LTDC_Cmd(ENABLE);
- LCD_SetLayer(LCD_FOREGROUND_LAYER);
- LCD_Clear(LCD_COLOR_BLACK);
- LCD_SetColors(LCD_COLOR_GREEN,LCD_COLOR_BLACK);
-
- while (1)
- {
- #if DATA_TYPE == 0
- Axis.AccX = getAccX();
- Axis.AccY = getAccY();
- Axis.AccZ = getAccZ();
- Axis.GyroX = getGyroX();
- Axis.GyroY = getGyroY();
- Axis.GyroZ = getGyroZ();
- printf("AX:%3.2f AY:%3.2f AZ:%3.2f GX:%4.0f GY:%4.0f GZ:%4.0f",
- Axis.AccX,
- Axis.AccY,
- Axis.AccZ,
- Axis.GyroX,
- Axis.GyroY,
- Axis.GyroZ);
- #if DATA_DISPLAY == 1
- sprintf(LCD_Buffer,"AX:%3.2f AY:%3.2f AZ:%3.2f ",
- Axis.AccX,
- Axis.AccY,
- Axis.AccZ);
- LCD_DisplayStringLine(1,LCD_Buffer);
-
- sprintf(LCD_Buffer,"GX:%4.0f GY:%4.0f GZ:%4.0f ",
- Axis.GyroX,
- Axis.GyroY,
- Axis.GyroZ);
- LCD_DisplayStringLine(21,LCD_Buffer);
- #endif
-
- #elif DATA_TYPE == 1
- printf("Pitch:%3.0f Roll:%3.0f Yaw:%3.0f",
- EulerAngle.Pitch,
- EulerAngle.Roll,
- EulerAngle.Yaw);
- #if DATA_DISPLAY == 1
- sprintf(LCD_Buffer,"Pitch:%3.0f Roll:%3.0f Yaw:%3.0f ",
- EulerAngle.Pitch,
- EulerAngle.Roll,
- EulerAngle.Yaw);
- LCD_DisplayStringLine(1,LCD_Buffer);
- #endif
-
- #elif DATA_TYPE == 2
- Gesture.Angle_FY = getFuYangAngle();
- Axis.GyroY = getGyroY();
- Kalman_Filter(Gesture.Angle_FY,Axis.GyroY,&FilterAngle);
- printf("FY1:%3.1f FY2:%3.1f",Gesture.Angle_FY,FilterAngle);
-
- #if DATA_DISPLAY == 1
- sprintf(LCD_Buffer,"FY1:%3.1f FY2:%3.1f",Gesture.Angle_FY,FilterAngle);
- LCD_DisplayStringLine(1,LCD_Buffer);
- #endif
-
- #endif
- GPIOG->ODR ^= GPIO_Pin_14;
- delayms(30);
- }
- }
|
|
