找回密碼
 立即注冊(cè)

QQ登錄

只需一步,快速開(kāi)始

帖子
查看: 6007|回復(fù): 1
收起左側(cè)

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

[復(fù)制鏈接]
ID:127084 發(fā)表于 2016-6-17 12:27 | 顯示全部樓層 |閱讀模式
臨近期末,學(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)自己谷歌一下

58a0574b86023741dfc90243535b5163.jpg
  1. /*-----------------------------------------------------------------------------------
  2.                                                                                                 STM32F429ZI+MPU6050+AHRS
  3.         
  4.         簡(jiǎn)介:
  5.         
  6.                         1) 如何計(jì)算積分
  7.                          對(duì)角速度積分得到角度,但是會(huì)累積誤差,dt為積分時(shí)間s秒,最好放在中斷運(yùn)行.中斷每隔
  8.                          dt秒進(jìn)入一次中斷并進(jìn)行計(jì)算,必須確保在1個(gè)中斷周期內(nèi)能夠完成全部計(jì)算.
  9.                         
  10.                          Example:                        
  11.                          Axis.GyroY = getGyroY();                        //獲取Y軸角速度,單位為度
  12.                          Angle += Axis.GyroY * dt;    //離散信號(hào)的積分即為求黎曼和.
  13.                
  14.                 2) 互補(bǔ)濾波器
  15.                          原理是取當(dāng)前傾角和加速度,做差獲得傾角差值并進(jìn)行放大,然后與陀螺儀角速度
  16.                          疊加后再積分,從而使傾角跟蹤為加速度獲得的角度.
  17.                          2為放大倍數(shù),可調(diào)節(jié)補(bǔ)償度;0.01為系統(tǒng)周期10ms.
  18.                         
  19.                          Example:                        
  20.                          FilterAngle = FilterAngle + (((Gesture.Angle_FY - FilterAngle)*2 + Axis.GyroY)*0.01);
  21.                
  22.                 3) 卡爾曼濾波器
  23.                          原理請(qǐng)百度
  24.                         
  25.                          Example:                        
  26.                          Kalman_Filter(Gesture.Angle_FY,Axis.GyroY,&FilterAngle);
  27.                         
  28.                 4) AHRS數(shù)據(jù)融合
  29.                          通過(guò)四元數(shù)矩陣運(yùn)算把加速度計(jì)和陀螺儀的數(shù)據(jù)進(jìn)行融合計(jì)算出歐拉角.
  30.                  
  31.         
  32. +------------------------+-----------------------+----------------------------+
  33. +                                               LCD GPIO分配表                                   +
  34. +------------------------+-----------------------+----------------------------+
  35. |  LCD_TFT R2 <-> PC10   |  LCD_TFT G2 <-> PA6   |  LCD_TFT B2 <-> PD6        |
  36. |  LCD_TFT R3 <-> PB0    |  LCD_TFT G3 <-> PG10  |  LCD_TFT B3 <-> PG11       |
  37. |  LCD_TFT R4 <-> PA11   |  LCD_TFT G4 <-> PB10  |  LCD_TFT B4 <-> PG12       |
  38. |  LCD_TFT R5 <-> PA12   |  LCD_TFT G5 <-> PB11  |  LCD_TFT B5 <-> PA3        |
  39. |  LCD_TFT R6 <-> PB1    |  LCD_TFT G6 <-> PC7   |  LCD_TFT B6 <-> PB8        |
  40. |  LCD_TFT R7 <-> PG6    |  LCD_TFT G7 <-> PD3   |  LCD_TFT B7 <-> PB9        |
  41. -------------------------------------------------------------------------------
  42.           |  LCD_TFT HSYNC <-> PC6   | LCDTFT VSYNC <->  PA4   |
  43.           |  LCD_TFT CLK   <-> PG7   | LCD_TFT DE   <->  PF10  |
  44.            -----------------------------------------------------

  45. +-------------------+--------------------+--------------------+--------------------+
  46. +                                               SDRAM GPIO 分配表                                     +
  47. +-------------------+--------------------+--------------------+--------------------+
  48. | PD0  <-> FMC_D2   | PE0  <-> FMC_NBL0  | PF0  <-> FMC_A0    | PG0  <-> FMC_A10   |
  49. | PD1  <-> FMC_D3   | PE1  <-> FMC_NBL1  | PF1  <-> FMC_A1    | PG1  <-> FMC_A11   |
  50. | PD8  <-> FMC_D13  | PE7  <-> FMC_D4    | PF2  <-> FMC_A2    | PG8  <-> FMC_SDCLK |
  51. | PD9  <-> FMC_D14  | PE8  <-> FMC_D5    | PF3  <-> FMC_A3    | PG15 <-> FMC_NCAS  |
  52. | PD10 <-> FMC_D15  | PE9  <-> FMC_D6    | PF4  <-> FMC_A4    |--------------------+
  53. | PD14 <-> FMC_D0   | PE10 <-> FMC_D7    | PF5  <-> FMC_A5    |   
  54. | PD15 <-> FMC_D1   | PE11 <-> FMC_D8    | PF11 <-> FMC_NRAS  |
  55. +-------------------| PE12 <-> FMC_D9    | PF12 <-> FMC_A6    |
  56.                      | PE13 <-> FMC_D10   | PF13 <-> FMC_A7    |   
  57.                      | PE14 <-> FMC_D11   | PF14 <-> FMC_A8    |
  58.                      | PE15 <-> FMC_D12   | PF15 <-> FMC_A9    |
  59. +-------------------+--------------------+--------------------+
  60. | PB5 <-> FMC_SDCKE1|
  61. | PB6 <-> FMC_SDNE1 |
  62. | PC0 <-> FMC_SDNWE |
  63. +-------------------+  
  64.   
  65.         作者: BoX
  66.         版本: V1.0
  67.         平臺(tái): STM32F429I-Discovery Board
  68.         日期: 2014年7月11日
  69. ------------------------------------------------------------------------------------*/
  70. #include "LED.h"
  71. #include "stm32f4_tft.h"
  72. #include "stm32f4_delay.h"
  73. #include "stm32f4_usart1.h"
  74. #include "stm32f4_timer.h"
  75. #include "stm32f4_ahrs.h"
  76. #include "stm32f4_mpu6050.h"
  77. #include "iic_analog.h"
  78. #include "kalman_filter.h"
  79. #include "stm32f4xx.h"
  80. /*------------------------------------------
  81.                                                                 條件編譯
  82.         DATA_TYPE
  83.         0 - 輸出處理后的數(shù)據(jù),加速度單位為g,角速度
  84.                   單位為deg/s
  85.         1 - 輸出數(shù)據(jù)融合后的歐拉角.
  86.         2 - 輸出卡爾曼濾波后的角度.
  87.         
  88.         DATA_DISPLAY
  89.         0 - 數(shù)據(jù)不顯示在LCD上
  90.         1 - 數(shù)據(jù)顯示在LCD上
  91. ------------------------------------------*/
  92. #define USAR_DELAY    (800)
  93. #define DATA_TYPE                        (1)
  94. #define DATA_DISPLAY  (1)
  95. /*------------------------------------------
  96.                                                                 全局變量
  97. ------------------------------------------*/
  98. extern MPU6050_AxisTypeDef    Axis;
  99. extern AHRS_EulerAngleTypeDef EulerAngle;

  100. uint8_t LCD_Buffer[240];
  101. /*------------------------------------------
  102.                                                                 主函數(shù)
  103. ------------------------------------------*/
  104. int main(void)
  105. {
  106.         DelayInit();
  107.         LED_Init();
  108.         USART1_Config();
  109.         printf(" AHRS OK!");
  110.         MPU6050_Init();
  111.         TIM2_Config(2000,89);  //每2ms溢出中斷為AHRS_Update提供定時(shí)
  112.         LCD_Init();
  113.         LCD_LayerInit();
  114.         LTDC_Cmd(ENABLE);
  115.         LCD_SetLayer(LCD_FOREGROUND_LAYER);
  116.         LCD_Clear(LCD_COLOR_BLACK);
  117.         LCD_SetColors(LCD_COLOR_GREEN,LCD_COLOR_BLACK);
  118.         
  119.         while (1)
  120.         {
  121. #if DATA_TYPE         == 0
  122.                 Axis.AccX  = getAccX();
  123.                 Axis.AccY  = getAccY();
  124.                 Axis.AccZ  = getAccZ();
  125.                 Axis.GyroX = getGyroX();
  126.                 Axis.GyroY = getGyroY();
  127.                 Axis.GyroZ = getGyroZ();
  128.                 printf("AX:%3.2f AY:%3.2f AZ:%3.2f GX:%4.0f GY:%4.0f GZ:%4.0f",
  129.                                                                                                                                                                                                                  Axis.AccX,
  130.                                                                                                                                                                                                                  Axis.AccY,
  131.                                                                                                                                                                                                                  Axis.AccZ,
  132.                                                                                                                                                                                                                  Axis.GyroX,
  133.                                                                                                                                                                                                                  Axis.GyroY,
  134.                                                                                                                                                                                                                  Axis.GyroZ);
  135.         #if DATA_DISPLAY == 1               
  136.                 sprintf(LCD_Buffer,"AX:%3.2f AY:%3.2f AZ:%3.2f  ",
  137.                                                                                                                                                                                                                  Axis.AccX,
  138.                                                                                                                                                                                                                  Axis.AccY,
  139.                                                                                                                                                                                                                  Axis.AccZ);
  140.                 LCD_DisplayStringLine(1,LCD_Buffer);
  141.                
  142.                 sprintf(LCD_Buffer,"GX:%4.0f GY:%4.0f GZ:%4.0f  ",
  143.                                                                                                                                                                                                                  Axis.GyroX,
  144.                                                                                                                                                                                                                  Axis.GyroY,
  145.                                                                                                                                                                                                                  Axis.GyroZ);
  146.                 LCD_DisplayStringLine(21,LCD_Buffer);
  147.         #endif
  148.         
  149. #elif DATA_TYPE         == 1
  150.                 printf("Pitch:%3.0f Roll:%3.0f Yaw:%3.0f",
  151.                                                                                                                                                                                                         EulerAngle.Pitch,
  152.                                                                                                                                                                                                         EulerAngle.Roll,
  153.                                                                                                                                                                                                         EulerAngle.Yaw);
  154.         #if DATA_DISPLAY == 1
  155.                 sprintf(LCD_Buffer,"Pitch:%3.0f Roll:%3.0f Yaw:%3.0f  ",
  156.                                                                                                                                                                                                                                         EulerAngle.Pitch,
  157.                                                                                                                                                                                                                                         EulerAngle.Roll,
  158.                                                                                                                                                                                                                                         EulerAngle.Yaw);
  159.                 LCD_DisplayStringLine(1,LCD_Buffer);
  160.         #endif
  161.         
  162. #elif DATA_TYPE         == 2
  163.                 Gesture.Angle_FY = getFuYangAngle();
  164.                 Axis.GyroY       = getGyroY();
  165.                 Kalman_Filter(Gesture.Angle_FY,Axis.GyroY,&FilterAngle);
  166.                 printf("FY1:%3.1f FY2:%3.1f",Gesture.Angle_FY,FilterAngle);
  167.                
  168.         #if DATA_DISPLAY == 1
  169.                 sprintf(LCD_Buffer,"FY1:%3.1f FY2:%3.1f",Gesture.Angle_FY,FilterAngle);
  170.                 LCD_DisplayStringLine(1,LCD_Buffer);
  171.         #endif
  172.         
  173. #endif                        
  174.                 GPIOG->ODR ^= GPIO_Pin_14;
  175.                 delayms(30);
  176.         }
  177. }
復(fù)制代碼

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

回復(fù)

使用道具 舉報(bào)

ID:94995 發(fā)表于 2019-4-2 14:28 | 顯示全部樓層
不錯(cuò)。學(xué)學(xué)
回復(fù)

使用道具 舉報(bào)

本版積分規(guī)則

小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表