STM32F429開發(fā)-空間姿態(tài)檢測系統(tǒng)

ID:127084 · 發(fā)表于 2016-6-17 12:27

臨近期末，學(xué)校剛剛考完試，弄了一個基于STM32F429的姿態(tài)檢測系統(tǒng)，也就是俗稱的AHRS，被很多人用來做四軸飛行器的，不過本人對四軸飛行器不是很感興趣，主要用來檢測人體姿態(tài)和仿生控制的。采用MPU6050進(jìn)行數(shù)據(jù)融合，得到空間姿態(tài)信息，從而完成手勢檢測，動作檢測，人體姿態(tài)檢測。基于MEMS的三軸陀螺儀，加速度計。目前AHRS內(nèi)部采用的多傳感器數(shù)據(jù)融合進(jìn)行的航姿解算單元為卡爾曼濾波器。
特點：高精度360 度全方位位置姿態(tài)輸出，數(shù)據(jù)融合算法快速動態(tài)響應(yīng)與長時間穩(wěn)定性（無漂移，無積累誤差）
輸出：三維全姿態(tài)數(shù)據(jù)（四元數(shù) / 歐拉角 / 旋轉(zhuǎn)矩陣/原始數(shù)據(jù)）三維加速度 / 三維角速度

不多說了上圖！相關(guān)原理請自己谷歌一下

/*-----------------------------------------------------------------------------------
STM32F429ZI+MPU6050+AHRS
簡介:
1) 如何計算積分
對角速度積分得到角度,但是會累積誤差,dt為積分時間s秒,最好放在中斷運(yùn)行.中斷每隔
dt秒進(jìn)入一次中斷并進(jìn)行計算,必須確保在1個中斷周期內(nèi)能夠完成全部計算.
Example:
Axis.GyroY = getGyroY(); //獲取Y軸角速度,單位為度
Angle += Axis.GyroY * dt; //離散信號的積分即為求黎曼和.
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) 卡爾曼濾波器
原理請百度
Example:
Kalman_Filter(Gesture.Angle_FY,Axis.GyroY,&FilterAngle);
4) AHRS數(shù)據(jù)融合
通過四元數(shù)矩陣運(yùn)算把加速度計和陀螺儀的數(shù)據(jù)進(jìn)行融合計算出歐拉角.
+------------------------+-----------------------+----------------------------+
+ 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 - 輸出處理后的數(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提供定時
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);
}
}

復(fù)制代碼

空間姿態(tài)檢測.zip (611.82 KB, 下載次數(shù): 58)

ID:94995 · 發(fā)表于 2019-4-2 14:28

不錯。學(xué)學(xué)

帳號		自動登錄	找回密碼
密碼			立即注冊

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

STM32F429開發(fā)-空間姿態(tài)檢測系統(tǒng)

相關(guān)帖子