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滤波。

扫描二维码关注公众号，回复： 15331494 查看本文章

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姿态解算 获取。