STM32F429开发-空间姿态检测系统

ID:127084 · 发表于 2016-6-17 12:27

临近期末，学校刚刚考完试，弄了一个基于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)

ID:94995 · 发表于 2019-4-2 14:28

不错。学学

帐号		自动登录	找回密码
密码			立即注册

STM32F429开发-空间姿态检测系统

相关帖子