|
臨近期末，學校剛剛考完試，弄了一個基于STM32F429的姿態檢測系統，也就是俗稱的AHRS，被很多人用來做四軸飛行器的，不過本人對四軸飛行器不是很感興趣，主要用來檢測人體姿態和仿生控制的。采用MPU6050進行數據融合，得到空間姿態信息，從而完成手勢檢測，動作檢測，人體姿態檢測。基于MEMS的三軸陀螺儀，加速度計。目前AHRS內部采用的多傳感器數據融合進行的航姿解算單元為卡爾曼濾波器。
特點：高精度360 度全方位位置姿態輸出，數據融合算法快速動態響應與長時間穩定性（無漂移，無積累誤差）
輸出：三維全姿態數據（四元數 / 歐拉角 / 旋轉矩陣/原始數據）三維加速度 / 三維角速度
不多說了上圖！相關原理請自己谷歌一下
- /*-----------------------------------------------------------------------------------
- STM32F429ZI+MPU6050+AHRS
-
- 簡介:
-
- 1) 如何計算積分
- 對角速度積分得到角度,但是會累積誤差,dt為積分時間s秒,最好放在中斷運行.中斷每隔
- dt秒進入一次中斷并進行計算,必須確保在1個中斷周期內能夠完成全部計算.
-
- Example:
- Axis.GyroY = getGyroY(); //獲取Y軸角速度,單位為度
- Angle += Axis.GyroY * dt; //離散信號的積分即為求黎曼和.
-
- 2) 互補濾波器
- 原理是取當前傾角和加速度,做差獲得傾角差值并進行放大,然后與陀螺儀角速度
- 疊加后再積分,從而使傾角跟蹤為加速度獲得的角度.
- 2為放大倍數,可調節補償度;0.01為系統周期10ms.
-
- Example:
- FilterAngle = FilterAngle + (((Gesture.Angle_FY - FilterAngle)*2 + Axis.GyroY)*0.01);
-
- 3) 卡爾曼濾波器
- 原理請百度
-
- Example:
- Kalman_Filter(Gesture.Angle_FY,Axis.GyroY,&FilterAngle);
-
- 4) AHRS數據融合
- 通過四元數矩陣運算把加速度計和陀螺儀的數據進行融合計算出歐拉角.
-
-
- +------------------------+-----------------------+----------------------------+
- + 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
- 平臺: 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 - 輸出處理后的數據,加速度單位為g,角速度
- 單位為deg/s
- 1 - 輸出數據融合后的歐拉角.
- 2 - 輸出卡爾曼濾波后的角度.
-
- DATA_DISPLAY
- 0 - 數據不顯示在LCD上
- 1 - 數據顯示在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];
- /*------------------------------------------
- 主函數
- ------------------------------------------*/
- int main(void)
- {
- DelayInit();
- LED_Init();
- USART1_Config();
- printf(" AHRS OK!");
- MPU6050_Init();
- TIM2_Config(2000,89); //每2ms溢出中斷為AHRS_Update提供定時
- 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);
- }
- }
空間姿態檢測.zip
(611.82 KB, 下載次數: 58)
2016-6-17 12:27 上傳
點擊文件名下載附件
|
