找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 4619|回复: 10
收起左侧

STM32F103ZE+mpu6050+hmc5883L通过卡尔曼滤波串口输出姿态角,数据经过融合

  [复制链接]
ID:125878 发表于 2019-7-10 10:44 | 显示全部楼层 |阅读模式
基于STM32F103ZE+mpu6050+hmc5883L通过卡尔曼滤波串口输出姿态角,数据经过融合,Z无漂移,绝对指向

单片机源程序如下:
  1. /* main.c file
  2. 功能:
  3. 1.初始化各个传感器,
  4. 2.运行姿态解算和高度测量
  5. 3.将解算的姿态和各个传感器的输出上传到 MiniIMU AHRS 测试软件
  6. 4.响应 PC发送的命令
  7. ------------------------------------
  8. */

  9. #include "common.h"  //包含所有的驱动 头文件

  10. //上传数据的状态机
  11. #define REIMU  0x01 //上传解算的姿态数据
  12. #define REMOV  0x02        //上传传感器的输出
  13. #define REHMC  0x03        //上传磁力计的标定值

  14. #define Upload_Speed  15   //数据上传速度  单位 Hz
  15. #define upload_time (1000000/Upload_Speed)/2  //计算上传的时间。单位为us

  16. int16_t ax, ay, az;       
  17. int16_t gx, gy, gz;
  18. int16_t hx, hy, hz;
  19. int32_t Temperature = 0, Pressure = 0, Altitude = 0;
  20. uint32_t system_micrsecond;
  21. int16_t hmcvalue[3];
  22. u8 state= REIMU;  //发送特定帧 的状态机
  23. /**************************实现函数********************************************
  24. *函数原型:                int main(void)
  25. *功  能:                主程序
  26. *******************************************************************************/
  27. int main(void)
  28. {
  29.         int16_t Math_hz=0;
  30.         unsigned char PC_comm; //PC 命令关键字节         
  31.         float ypr[3]; // yaw pitch roll
  32.         /* 配置系统时钟为72M 使用外部8M晶体+PLL*/      
  33.     SystemInit();
  34.         delay_init(72);                //延时初始化
  35.     Initial_LED_GPIO();        //初始化STM32-SDK板子上的LED接口
  36.         Initial_PWMLED();
  37.         Initial_UART1(115200L);
  38.         Initial_UART2(115200L);
  39.         IIC_Init();         //初始化I2C接口
  40.         delay_ms(300);        //等待器件上电
  41.         //UART1_Put_String("Initialize...\r\n");
  42.         AHRS_init(); //初始化IMU和传感器
  43.         system_micrsecond=micros();

  44.         while(1){        //主循环
  45.                
  46.         //delay_ms(1); //延时,不要算那么快。
  47.         AHRS_getYawPitchRoll(ypr); //姿态更新
  48.         printf("%f %f %f\r\n",ypr[1],ypr[2],ypr[0]);
  49.         Math_hz++; //解算次数 ++
  50.         BMP180_Routing(); //处理BMP018 事务 开启转换和读取结果将在这个子程序中进行

  51. //-------------上位机------------------------------
  52.         //是否到了更新 上位机的时间了?
  53.         if((micros()-system_micrsecond)>upload_time){
  54.         switch(state){
  55.         case REIMU:
  56.         BMP180_getTemperat(&Temperature); //读取最近的温度值
  57.         BMP180_getPress(&Pressure);           //读取最近的气压测量值
  58.         BMP180_getAlt(&Altitude);           //读取相对高度
  59.         //UART1_ReportIMU((int16_t)(ypr[0]*10.0),(int16_t)(ypr[1]*10.0),
  60.         //(int16_t)(ypr[2]*10.0),Altitude/10,Temperature,Pressure/10,Math_hz*16);
  61.         //UART2_ReportIMU((int16_t)(ypr[0]*10.0),(int16_t)(ypr[1]*10.0),
  62.         //(int16_t)(ypr[2]*10.0),Altitude/10,Temperature,Pressure/10,Math_hz*Upload_Speed);
  63.        
  64.         Math_hz=0;
  65.         state = REMOV; //更改状态。
  66.         break;
  67.         case REMOV:
  68.         MPU6050_getlastMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  69.         HMC58X3_getlastValues(&hx,&hy,&hz);
  70.         //UART1_ReportMotion(ax,ay,az,gx,gy,gz,hx,hy,hz);
  71.         //UART2_ReportMotion(ax,ay,az,gx,gy,gz,hx,hy,hz);
  72.         state = REIMU;
  73.         break;
  74.         default:
  75.         state = REIMU;
  76.         break;
  77.         }//switch(state)                          
  78.         system_micrsecond=micros();         //取系统时间 单位 us
  79.         LED_Change();        //LED1改变亮度
  80.         }
  81. //--------------------------------------------------
  82.         //处理PC发送来的命令
  83.         if((PC_comm=UART2_CommandRoute())!=0xff)
  84.         {
  85.         switch(PC_comm){ //检查命令标识
  86.         case Gyro_init:                        MPU6050_InitGyro_Offset(); break; //读取陀螺仪零偏
  87.         case High_init:                        BMP180_ResetAlt(0);         break;                //气压高度 清零
  88.         }
  89.         }// 处理PC 发送的命令

  90.         }//主循环 while(1) 结束

  91. }  //main       

  92. //------------------End of File----------------------------
复制代码

所有资料51hei提供下载:
Kalman_AHRS.7z (308.96 KB, 下载次数: 291)

评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励!

查看全部评分

回复

使用道具 举报

ID:479281 发表于 2019-7-24 18:38 | 显示全部楼层
大神为什么我下载你的程序烧录进32单片机读取的YAW水平转动没变化的1,维持在135度左右,roll和pitch翻转的时候正常变化,而YAW角度在翻转的时候也才有变化
回复

使用道具 举报

ID:479281 发表于 2019-7-24 18:39 | 显示全部楼层
楼主,为什么YAW水平转动数据没变化的?
回复

使用道具 举报

ID:479281 发表于 2019-7-25 10:07 | 显示全部楼层
wuhuojian 发表于 2019-7-24 18:39
楼主,为什么YAW水平转动数据没变化的?

找到问题了,指南针的线短路了,但旋转时候发现水平方向的角度并不均匀,不知道是不是HMC5883没校准的问题
回复

使用道具 举报

ID:479281 发表于 2019-7-25 17:18 | 显示全部楼层
YAW的数据还是会很缓慢地漂移,大概半个小时差一度左右,怎么解决呢,楼主
回复

使用道具 举报

ID:282431 发表于 2019-7-25 17:50 | 显示全部楼层
这个实用呀,好好学习。点赞点赞点赞
回复

使用道具 举报

ID:160072 发表于 2020-2-24 23:00 | 显示全部楼层
这个真的厉害 谢谢大神
回复

使用道具 举报

ID:457898 发表于 2020-5-13 12:21 | 显示全部楼层
请问这个程序中有原理图呢?
回复

使用道具 举报

ID:457898 发表于 2020-5-13 12:37 | 显示全部楼层
请问里面有原理图吗?
回复

使用道具 举报

ID:538585 发表于 2020-5-14 16:20 | 显示全部楼层
可以姿态解算,但是不知道为什么,用起来完全不行。整个姿态角的误差特别大。不知是什么原因?
回复

使用道具 举报

ID:726668 发表于 2020-7-12 16:47 | 显示全部楼层
这没有看到用Kalman滤波啊,用的是互补滤波求解四元素,然后反求欧拉角。。。。
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表