FPGA实现MPU6050姿态解算---附完整代码

这篇具有很好参考价值的文章主要介绍了FPGA实现MPU6050姿态解算---附完整代码。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

一. 简介

在之前的文章中(很久之前了(CSND中)),已经通过FPGA获取到了MPU6050的六轴数据: 三轴加速 和 三轴角速度,但是没有对它进行然后处理。那么在本篇文章中,将利用Cordic算法来进行姿态解算。

二. 踩坑分享

在进行姿态解算分享之前,先分享一个踩坑经历。一般来说MPU6050的ID读出为0x68,淘宝上买到的模块,基本上都是这个。但是我使用的是自己画的PCB,手动焊接的,在读取ID的时候,一直为0x98,但是认知中要为0x68才是对的,这个时候就会怀疑是不是自己的程序或者焊接的问题了。但好在后面读取六轴数据,姿态解算后得到的角度基本是正确的(折腾了一天了,才发现)。这是个坑,大家可以注意一下。

三. 姿态解算

所谓姿态解算就是通过六轴的数据,来求解物体的三个角度: roll , pitch , yaw。

1. 通过加速度求解

先看一下加速度求解角度的表达式(通过加速度是无法求解yaw的)。atan(acc_y / acc_x)和sqrt(acc_y*acc_y + acc_z * acc_z)不就是上篇中Cordic算法求解的值吗,都不需要使用的其他的计算。

roll = atan(acc_y / acc_x);
pitch = atan(acc_x / (sqrt(acc_y*acc_y + acc_z * acc_z)));
2. 通过角速度求解

通过角速度的求解就更简单了,只需要将当前角度加上(角速度×dt)就可以。角速度求解的时候会有些问题,在静态的时候,角速度会有零漂,这个时候角度误差会越来越大。

3. 融合

可以看到有上面的两种方法求解角度,可以单独使用,但是可能会不太准确,精度要求不高的场合可以只使用加速度求解。在精度要求比较高的场合下,需要使用这两种方法求解,然后再将求得的结果进行融合。常用的方法有: 卡尔曼滤波、一阶互补滤波、二阶互补滤波。

一阶互补滤波,如下,简单粗暴。要想滤波效果好的话,可以试试卡尔曼滤波。

roll = a * acc_roll + (1 - a) *gyro_roll;

以上只是一种比较常规的求解方法,追求高精度的话,可以使用四元数的方法进行求解(复杂度大大增加)。

四. 代码实现

代码都是现成的,在之前的文章中已经写好了,这里做的工作就是将这些模块组合在一起。

1. 模块接口

输入请求,输出应答和三个角度,角度值扩大了2^16倍。

module IMU(
    input                       clk,       //27M
    input                       rst_n,

    input                       imu_req,
    output                      imu_ack,

    output signed[31:0]         roll,
    output signed[31:0]         pitch,
    output signed[31:0]         yaw,

    output                      IICSCL,             /*IIC 时钟输出*/
    inout                       IICSDA             /*IIC 数据线*/ 
);

2. 状态机

这里使用到了两个Cordic模块,第一个模块先计算出roll和sqrt(acc_yacc_y + acc_z * acc_z)的值,然后第二个模块通过acc_x和sqrt(acc_yacc_y + acc_z * acc_z)的值 计算出 pitch的角度。最后对数据经过了一个简单的FIR滤波。

always@(*) begin
    case(state)
    S_IDLE:
        if( imu_req == 1'b1)
            next_state <= S_READ_MPU6050;
        else
            next_state <= S_IDLE;
    S_READ_MPU6050:
        if( mpu6050_ack == 1'b1 )
            next_state <= S_Cordic;
        else
            next_state <= S_READ_MPU6050;
    S_Cordic:
        if( cordic_ack == 1'b1)
            next_state <= S_Cordic2;
        else
            next_state <= S_Cordic;
    S_Cordic2:
        if( cordic2_ack == 1'b1)
            next_state <= S_FILTER;
        else
            next_state <= S_Cordic2;
    S_FILTER:
        if( fir_filter_ack == 1'b1)
            next_state <= S_ACK;
        else
            next_state <= S_FILTER;
    S_ACK:
        next_state <= S_IDLE;
    default:     next_state <= S_IDLE;
    endcase
end
3. 融合

这里的融合,暂时没有做,只对加速度求解的角度进行了一个滤波处理,后面会根据需要再进行更新。

FIR_Filter FIR_Filter_HP(

    .clk                        (           clk             ),
    .rst_n                      (           rst_n           ),

    .fir_filter_req             (           fir_filter_req  ),
    .fir_filter_ack             (           fir_filter_ack  ),

    .filter_data_in             (           theta           ),
    .filter_data_out            (           acc_roll        )
);


FIR_Filter FIR_Filter_HP2(

    .clk                        (           clk             ),
    .rst_n                      (           rst_n           ),

    .fir_filter_req             (           fir_filter_req  ),
    .fir_filter_ack             (                           ),

    .filter_data_in             (           theta2           ),
    .filter_data_out            (           acc_pitch        )
);

模块已上板测试,解算出来的角度没有问题(可能精度不是那么完美,滤波与融合那里需要下点功夫)。

欢迎交流学习!!!

需要完整代码的 可以关注微信公众号回复 FPGA_MPU6050姿态解算 获取。文章来源地址https://www.toymoban.com/news/detail-806085.html

到了这里,关于FPGA实现MPU6050姿态解算---附完整代码的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 【ESP32Arduino+MPU6050姿态解算】自制无人机学习笔记2 PLatformIO 下载即可使用

    本人之前发表过一篇关于esp32读取mpu6050数据的文章,链接:http://t.csdn.cn/AwzSZ,但其存在一些漏洞,具体表现在输出数据存在不连贯和错误,在mpu6050高速运动时存在较大误差等。仅作为参考。故在此重发作为修正。当前该篇文章中所述的模块,已经过无人机稳定性控制的测试

    2024年02月16日
    浏览(41)
  • 【无人机/平衡车/机器人】详解STM32+MPU6050姿态解算—卡尔曼滤波+四元数法+互补滤波(文末附3个算法源码)

     效果: MPU6050姿态解算-卡尔曼滤波+四元数+互补滤波 目录 基础知识详解 欧拉角

    2024年04月14日
    浏览(69)
  • FPGA实现卡尔曼滤波算法——融合MPU6050的Acc和Gyro

    参考GITHUB大神DOA.c作品: 原理回顾: MPU6050传感器原理: 陀螺仪存在静态误差(积分运算导致),加速度计存在动态误差(重力加速度g),因此需要通过数据融合来消除误差,得到理想数据。Mpu6050是六轴姿态传感器,包括三轴Acc和三轴Gyro,gyro采样频率8000HZ,acc采样频率10

    2024年03月25日
    浏览(56)
  • 单片机开发教程3——串口发送MPU6050姿态角

    MPU6050 是 InvenSense 公司推出的整合性 6 轴运动处理组件,其内部整合了 3 轴陀螺仪和 3 轴加速度传感器,并且含有一个IIC 接口, 可用于连接外部磁力传感器,并利用自带的数字运动处理器(DMP: Digital Motion Processor) 硬件加速引擎,通过主 IIC 接口,向应用端输出完整的 9 轴融

    2024年02月14日
    浏览(40)
  • 姿态传感器——MPU6050

    MPU6050是由三个陀螺仪和三个加速度传感器组成的 6轴运动处理组件 ,是一款六轴(三轴加速度+三轴角速度(陀螺仪))传感器。 · 内部主要结构 陀螺仪 、 加速度计 、 数字运动处理器DMP (Digital Motion Processor) MPU6050含有两个IIC接口,第一IIC接口可作为主接口给单片机传输

    2024年02月02日
    浏览(48)
  • 【STM32学习】——I2C通信协议&MPU6050姿态传感器&软件I2C读写MPU6050

    ​   目录 前言 一、I2C通信协议 1.简介 2.硬件电路设计 3.I2C时序(软件)

    2024年02月16日
    浏览(51)
  • STM32F103C8驱动MPU6050姿态与tofsense报警 (六)

    主函数 int main(void)  {         //RCC_Configuration(); //时钟设置          //BUZZER_BEEP1();//蜂鸣器音1         //BUZZER_BEEP1();//蜂鸣器音1     //delay_ms(50);     SYS_Init();//系统初始化总函数     while(1)   //主循环     {      // BUZZER_BEEP1();//蜂鸣器音1         MPU_Read();    //MP

    2024年01月17日
    浏览(76)
  • STM32F103C8驱动MPU6050姿态与tofsense报警 (一)

    本工程是实现STM32F103C8获取 mpu6050欧拉角(pitch ,roll,yow) mpu6050自带的dmp  第一步:设置串口 #if EN_USART1_RX   //如果使能了接收 //串口1中断服务程序 //注意,读取USARTx-SR能避免莫名其妙的错误        u8 USART_RX_BUF[USART_REC_LEN];     //接收缓冲,最大USART_REC_LEN个字节. //接收状态 //bit15,

    2024年01月17日
    浏览(42)
  • 一起玩儿物联网人工智能小车(ESP32)——71 姿态传感器MPU6050的使用方法

    摘要:本文介绍姿态传感器MPU6050的使用方法 姿态传感器是一种用于测量物体姿态的传感器,它可以检测物体的加速度和角速度,并计算出物体的方向和角度。前边介绍的RPI-1031只能定性的得到物体的倾斜情况,而姿态传感器则可以定量的得到物体倾斜的方向和角度。 MPU6050是

    2024年04月28日
    浏览(40)
  • mpu6050卡尔曼滤波C语言代码

    写在前面:目前网上很多卡尔曼滤波的c语言代码有一些小问题,且不方便移植,写这个博客的目前是想提供一个直接复制粘贴就能使用的c语言代码,理论推导部分请参考别的博客。

    2024年02月16日
    浏览(40)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包