京微齐力：基于H7的平衡控制系统（一、姿态解析）

前言

很久之前，就想用纯FPGA做一套控制系统。可以用到平衡车、飞控、水陆两栖船上面，让很多想快速入门比赛的学子，能够在这套“底盘”上面，结合图像处理、多信息融合等技术，快速搭建出自己的作品。恰逢认识FPGA之旅的作者-吴工，他也在做这件事，顿感追攀更觉相逢晚，恨不相逢早。对未来的真正慷慨，是把一切都献给现在，不再想，今天就开始做！

一、关于平衡控制系统

我们惊叹舞蹈演员在舞台上飞快地旋转，而身体却不会倾倒；我们佩服体操运动员一连翻几个筋斗，却能稳稳落地。如果我们不注意被石头绊着时，我们会“下意识”地立刻纠正身体的姿势，不让自己轻易摔倒。我们体内有一套维持身体平衡的器官系统在默默地为我们工作着。
在这里插入图片描述

人既是如此，那么机器也是这样的，机器要保持机身平衡，也需要这样一个器官系统来修正机器的运动。这种器官有很多，姿态传感器就是其中一种。目前主流的姿态传感器有三轴、六轴及九轴三种。这九轴分别就是：三轴加速度计，三轴陀螺仪以经三轴磁场。
作为本系列文章的开篇，本次实验，先选用较为中等的六轴-MPU6050作为姿态传感器，后面会慢慢根据系统来升级传感器。MPU6050由三轴加速度计和三轴陀螺仪组成，它可以测量物体在x、y、z三个方向上的加速度和角速度。加速度计可以检测物体的线性加速度，而陀螺仪可以检测物体的角速度。通过将加速度计和陀螺仪的测量结果进行组合，可以计算出物体的方向和角度。

关于六轴传感器的坐标系分析，网上很多，本文就不做赘续，有兴趣的可以自己查一查。

二、实验效果

1、FPGA采集MPU6050的数据，并进行滤波；
2、FPGA以串口的方式，将姿态数据发送到上位机。

基于H7的控制平衡系统（一）

从视频可以看到，当MPU6050姿态发生变化时，上位机波形跟传感器变化一致。yaw需要做角速度求解，这里只做了相对开始位置的，即初始值，所以只有在开始可以看到一点，后面的波形看不到。（具体原因请看第四节：理论简述）
在这里插入图片描述

三、硬件选择

1、H7P20N0L176-M2H1

本次实验使用H7作为主控板，HME-H7系列采用低功耗22nm 技术，集成了高性能ARM Cortex-M3 MCU(频率高达300M）、外围设备与大容量片上SRAM。本次实验只使用逻辑部分，后面根据需要再扩展MCU实验。需要板子的同学，可以到米联客店铺购买。
在这里插入图片描述

2、MPU6050

MPU6050采用I2C总线通信协议，可以直接连接到微控制器或单片机上。在使用MPU6050之前，需要进行校准，以保证其测量结果的准确性。校准过程可以通过软件或硬件进行。将VCC、GND、SCL、SDA连接到H7开发板即可。
在这里插入图片描述

MPU6050框图

四、理论简述

      物体的姿态，通俗的讲，就是通过六轴数据求解三个角度：
roll：绕X轴（横滚）范围：±180° ，与旋转方向相反转是增大 – 右滚为正，左滚为负；
pitch：绕Y轴（俯仰）范围：±90°,与旋转方向相反转是增大-- 抬头为正，低头为负；
yaw：绕z轴（偏航）范围：±180° ，与旋转方向相反转是增大 --右偏为正，左偏为负。
在这里插入图片描述
      当我们得到MPU6050的原始数据时，接下来如果我们要真正用上这些数据，通常我们都会利用数学方法把它们转换成角度。
      1、加速度求解角度的表达式
注：通过加速度是无法求解yaw的，因为重力加速度的Z轴，在相对地平面东西南北旋转时并无变化，因此只能靠Z轴的角速度来惯性累计估算旋转角度。


roll = atan(acc_y / acc_x);
pitch = atan(acc_x / (sqrt(acc_y*acc_y + acc_z * acc_z)));

      2、通过角速度求解：
      通过角速度的求解就更简单了，只需要将当前角度加上(角速度×dt)就可以。角速度求解的时候会有些问题，在静态的时候，角速度会有零漂，这个时候角度误差会越来越大。
      可以看到有上面的两种方法求解角度，可以单独使用，但是可能会不太准确，精度要求不高的场合可以只使用加速度求解。在精度要求比较高的场合下，需要使用这两种方法求解，然后再将求得的结果进行融合。常用的方法有: 卡尔曼滤波、清华角度滤波、一阶互补滤波、四元数解算。

这几种方法中，从难度上来看，一阶互补滤波和清华角度滤波是比较容易理解的，而且它们的本质其实是相同的，都是利用了权重互补，它们调试起来比较简单，而卡尔曼滤波和四元数解算的方法比较难理解。当然利用DMP直接输出角度也是可以的，不过移植起来也不太容易。从滤波效果上来看，本人的理解是：DMP直接输出角度>卡尔曼滤波>=四元数解算>清华角度滤波>=一阶互补滤波。不过其实一阶互补滤波只要把调试得比较好，得到的角度还是够用的。

一阶互补滤波：

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

五、程序设计

1、Cordic算法

求解：


roll = atan(acc_y / acc_x);
pitch = atan(acc_x / (sqrt(acc_y*acc_y + acc_z * acc_z)));

单片机或者FPGA等计算能力弱的嵌入式设备进行加减运算还是容易实现，但是想要计算三角函数（sin、cos、tan），甚至双曲线、指数、对数这样复杂的函数，那就需要费些力了。通常这些函数的计算需要通者查找表或近似计算（如泰勒级数逼近）等技术来转换为硬件易于实现的方式。

      Cordic(Coordinate Rotation Digital Computer, 坐标旋转数字计算方法)算法就是一种化繁为简的算法，通过基本的加减和移位运算代替乘法运算，逐渐逼近目标值，得出函数的数值解。
      具体实现理论可参考以下文章，这里不过多赘述：
      https://blog.csdn.net/ngany/article/details/117401494（CORDIC算法详解及FPGA实现）
      https://zhuanlan.zhihu.com/p/471677202（FPGA的算法解析4：CORDIC 算法解析）

`timescale 1ns / 1ps
//**************************************** Message ***********************************//
//技术交流：bumianzhe@126.com
//关注CSDN博主：“千歌叹尽执夏”
//Author: FPGA之旅 & 千歌叹尽执夏 
//All rights reserved	                               
//----------------------------------------------------------------------------------------
// Target Devices: 		H7P20N0L176-M2H1
// Tool Versions:       Fuxi 2023.1
// File name:           Cordic_arctan
// Last modified Date:  2023年8月25日16:00:00
// Last Version:        V1.1
// Descriptions:        Cordic算子
//----------------------------------------------------------------------------------------
//****************************************************************************************//


module Cordic_arctan(

    input           clk,
    input           rst_n,

    input           cordic_req,
    output          cordic_ack,

    input signed[15:0]  X,
    input signed[15:0]  Y,

    output[15:0]            amplitude,  //幅度，偏大1.64倍，这里做了近似处理
    output  signed[31:0]    theta    //扩大了2^16
);


`define rot0  32'd2949120       //45度*2^16
`define rot1  32'd1740992       //26.5651度*2^16
`define rot2  32'd919872        //14.0362度*2^16
`define rot3  32'd466944        //7.1250度*2^16
`define rot4  32'd234368        //3.5763度*2^16
`define rot5  32'd117312        //1.7899度*2^16
`define rot6  32'd58688         //0.8952度*2^16
`define rot7  32'd29312         //0.4476度*2^16
`define rot8  32'd14656         //0.2238度*2^16
`define rot9  32'd7360          //0.1119度*2^16
`define rot10 32'd3648          //0.0560度*2^16
`define rot11 32'd1856          //0.0280度*2^16
`define rot12 32'd896           //0.0140度*2^16
`define rot13 32'd448           //0.0070度*2^16
`define rot14 32'd256           //0.0035度*2^16
`define rot15 32'd128           //0.0018度*2^16




reg signed[31:0]    Xn[16:0];
reg signed[31:0]    Yn[16:0];
reg signed[31:0]    Zn[16:0];
reg[31:0]           rot[15:0];
reg                 cal_delay[16:0];


assign cordic_ack = cal_delay[16];
assign theta      = Zn[16];
assign amplitude  = ((Xn[16] >>> 1) + (Xn[16]  >>> 3) +(Xn[16] >>> 4)) >>> 16;  幅度，偏大1.64倍，这里做了近似处理 ,然后缩小了2^16

always@(posedge clk)
begin
    rot[0] <= `rot0;
    rot[1] <= `rot1;
    rot[2] <= `rot2;
    rot[3] <= `rot3;
    rot[4] <= `rot4;
    rot[5] <= `rot5;
    rot[6] <= `rot6;
    rot[7] <= `rot7;
    rot[8] <= `rot8;
    rot[9] <= `rot9;
    rot[10] <= `rot10;
    rot[11] <= `rot11;
    rot[12] <= `rot12;
    rot[13] <= `rot13;
    rot[14] <= `rot14;
    rot[15] <= `rot15;
end

always@(posedge clk or negedge rst_n)
begin
    if( rst_n == 1'b0)
        cal_delay[0] <= 1'b0;
    else
        cal_delay[0] <= cordic_req;
end

genvar j;
generate
    for(j = 1 ;j < 17 ; j = j + 1)
    begin: loop
        always@(posedge clk or negedge rst_n)
        begin
            if( rst_n == 1'b0)
                cal_delay[j] <= 1'b0;
            else
                cal_delay[j] <= cal_delay[j-1];
        end
    end
endgenerate

//将坐标挪到第一和四项限中
always@(posedge clk or negedge rst_n)
begin
    if( rst_n == 1'b0)
    begin
        Xn[0] <= 'd0;
        Yn[0] <= 'd0;
        Zn[0] <= 'd0;
    end
    else if( cordic_req == 1'b1)
    begin
        if( X < $signed(0) && Y < $signed(0))
        begin
            Xn[0] <= -(X << 16);
            Yn[0] <= -(Y << 16);
        end
        else if( X < $signed(0) && Y > $signed(0))
        begin
            Xn[0] <= -(X << 16);
            Yn[0] <= -(Y << 16);
        end
        else
        begin
            Xn[0] <= X << 16;
            Yn[0] <= Y << 16;
        end
        Zn[0] <= 'd0;
    end
    else 
    begin
        Xn[0] <= Xn[0];
        Yn[0] <= Yn[0];
        Zn[0] <= Zn[0];
    end
end


//旋转
genvar i;
generate
    for( i = 1 ;i < 17 ;i = i+1)
    begin: loop2
        always@(posedge clk or negedge rst_n)
        begin
            if( rst_n == 1'b0)
            begin
                Xn[i] <= 'd0;
                Yn[i] <= 'd0;
                Zn[i] <= 'd0;
            end
            else if( cal_delay[i -1] == 1'b1)
            begin
                if( Yn[i-1][31] == 1'b0)
                begin
                    Xn[i] <= Xn[i-1] + (Yn[i-1] >>> (i-1));
                    Yn[i] <= Yn[i-1] - (Xn[i-1] >>> (i-1));
                    Zn[i] <= Zn[i-1] + rot[i-1];
                end
                else
                begin
                    Xn[i] <= Xn[i-1] - (Yn[i-1] >>> (i-1));
                    Yn[i] <= Yn[i-1] + (Xn[i-1] >>> (i-1));
                    Zn[i] <= Zn[i-1] - rot[i-1];
                end
            end
            else
            begin
                Xn[i] <= Xn[i];
                Yn[i] <= Yn[i];
                Zn[i] <= Zn[i];
            end
        end
    end
endgenerate



endmodule

2、MPU6050采集数据

      MPU6050模块的接口是IIC，所以驱动的实质也是通过IIC协议对模块进行读写，和OLED模块一样。其流程为：
      初试话相关寄存器，例如角速度和加速度的精度。
      读取MPU6050模块的ID，判断是否初始化完成。
      角速度和加速度的数据读取。

`timescale 1ns / 1ps
//**************************************** Message ***********************************//
//技术交流：bumianzhe@126.com
//关注CSDN博主：“千歌叹尽执夏”
//Author: FPGA之旅 & 千歌叹尽执夏 
//All rights reserved	                               
//----------------------------------------------------------------------------------------
// Target Devices: 		H7P20N0L176-M2H1
// Tool Versions:       Fuxi 2023.1
// File name:           MPU6050_TOP
// Last modified Date:  2023年5月09日19:41:57
// Last Version:        V1.1
// Descriptions:        MPU6050数据采集
//----------------------------------------------------------------------------------------
//****************************************************************************************//


module MPU6050_TOP(

    input                       clk,
    input                       rst_n,

 
    input                       mpu6050_req,
    output                      mpu6050_ack,

    
    output   signed [15:0]      GYROXo,
    output   signed [15:0]      GYROYo,
    output   signed [15:0]      GYROZo,

    output   signed [15:0]      ACCELXo,
    output   signed [15:0]      ACCELYo,
    output   signed [15:0]      ACCELZo,


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



localparam      S_IDLE          =   'd0;
localparam      S_READ_GYRO     =   'd1;
localparam      S_READ_ACCEL    =   'd2;  
localparam      S_ACK           =   'd3;

reg[3:0]    state , next_state;
	
//读取角速度
wire		ReadGYROReq;
wire signed[15:0]	GYROX;
wire signed[15:0]	GYROY;
wire signed[15:0]	GYROZ;
wire	    GYRODone;

//读取加速度
wire		ReadACCELReq;
wire signed[15:0]	ACCELX;
wire signed[15:0]	ACCELY;
wire signed[15:0]	ACCELZ;
wire		ACCELDone;	


assign      mpu6050_ack = (state == S_ACK) ? 1'b1 : 1'b0;


//减去零漂
assign      GYROXo      = (GYROX >>> 2 ) - $signed(1 ) ;//  - $signed(37))  >>> 2;
assign      GYROYo      = (GYROY >>> 2 ) + $signed(6 ) ;//  + $signed(198)) >>> 2;
assign      GYROZo      = (GYROZ >>> 2 ) + $signed(1 ) ;//  + $signed(14))  >>> 2;



assign      ACCELXo     =   ACCELX ; //需要归一的+-8g ，所以需要除以一个4096，为了保留精度，指除以256
assign      ACCELYo     =   ACCELY ; //需要归一的+-8g ，所以需要除以一个4096，为了保留精度，指除以256
assign      ACCELZo     =   ACCELZ ; //需要归一的+-8g ，所以需要除以一个4096，为了保留精度，指除以256



assign      ReadGYROReq = ( state == S_READ_GYRO) ? 1'b1 : 1'b0;
assign      ReadACCELReq = ( state == S_READ_ACCEL) ? 1'b1 : 1'b0;

always@(posedge clk or negedge rst_n) begin
    if( rst_n == 1'b0)
        state <= S_IDLE;
    else
        state <= next_state;
end

always@(*)begin
    case(state)
    S_IDLE:
        if( mpu6050_req == 1'b1 )
            next_state <= S_READ_GYRO;
        else
            next_state <= S_IDLE;
    S_READ_GYRO:
        if( GYRODone == 1'b1)
            next_state <= S_READ_ACCEL;
        else
            next_state <= S_READ_GYRO;
    S_READ_ACCEL:
        if( ACCELDone == 1'b1)
            next_state <= S_ACK;
        else
            next_state <= S_READ_ACCEL;
    S_ACK:
        next_state <= S_IDLE;
    default: next_state <= S_IDLE;
    endcase
end



MPU6050_Read    u_MPU6050_Read(
	
	.clk                        (       clk         ),
	.rst                        (       rst_n       ),

	.SCL                        (       IICSCL      ),
	.SDA                        (       IICSDA      ),
	
	
	//读取角速度
	.ReadGYROReq                (       ReadGYROReq ),
	.GYROX                      (       GYROX       ),
	.GYROY                      (       GYROY       ),
	.GYROZ                      (       GYROZ       ),
	.GYRODone                   (       GYRODone    ),
	
	//读取加速度
	.ReadACCELReq               (       ReadACCELReq),
	.ACCELX                     (       ACCELX      ),
	.ACCELY                     (       ACCELY      ),
	.ACCELZ                     (       ACCELZ      ),
	.ACCELDone                  (       ACCELDone   )	
);


endmodule

3、fir&iir滤波

MPU6050芯片提供的数据夹杂有较严重的噪音，在芯片处理静止状态时数据摆动都可能超过2%。除了噪音，各项数据还会有偏移的现象，也就是说数据并不是围绕静止工作点摆动，因此要先对数据偏移进行校准，再通过滤波算法消除噪音。
有关fir&iir滤波器的相关理论请参照一下文章，这里不做过多赘述：
https://www.zhihu.com/question/323353814（如何通俗易懂地理解FIR/IIR滤波器？）

fir滤波：加速度滤波
加速度信号通常包含相对较低频率的变化，因为它主要反映物体的线性运动。在这种情况下，FIR滤波器可能更适合，因为它可以提供较好的静态响应，对于低频信号的滤波效果较好。

`timescale 1ns / 1ps
//**************************************** Message ***********************************//
//技术交流：bumianzhe@126.com
//关注CSDN博主：“千歌叹尽执夏”
//Author: FPGA之旅 & 千歌叹尽执夏 
//All rights reserved	                               
//----------------------------------------------------------------------------------------
// Target Devices: 		H7P20N0L176-M2H1
// Tool Versions:       Fuxi 2023.1
// File name:           MPU6050_fir_filter
// Last modified Date:  2023年12月9日13:46:00
// Last Version:        V1.1
// Descriptions:        fir滤波器
//----------------------------------------------------------------------------------------
//****************************************************************************************//


module MPU6050_fir_filter(
    input                   sys_clk_i                   ,   //系统时钟
    input                   sys_rst_n_i                 ,   //系统复位

    input                   fir_filter_req_i            ,   //fir滤波请求
    output                  fir_filter_ack_o            ,   //fir滤波响应

    
    input signed[15:0]      data_i                      ,   //输入待滤波数据
    output reg signed[15:0] filter_data_o                   //滤波后的数据

);

reg fir_filter_en;

reg[4:0] fir_filter_step_cnt;
//滤波器系数 扩大了255倍
localparam COE_0 = $signed(-5);//16'h04f0;
localparam COE_1 = $signed(-8);//16'hf410;
localparam COE_2 = $signed(20);//16'hf334;
localparam COE_3 = $signed(81);//16'h2587;
localparam COE_4 = $signed(115);//16'h4b72;
localparam COE_5 = $signed(81);//16'h2587;
localparam COE_6 = $signed(20);//16'hf334;
localparam COE_7 = $signed(-8);//16'hf410;
localparam COE_8 = $signed(-5);//16'h04f0;


reg signed[15:0] data_save0;
reg signed[15:0] data_save1;
reg signed[15:0] data_save2;
reg signed[15:0] data_save3;
reg signed[15:0] data_save4;
reg signed[15:0] data_save5;
reg signed[15:0] data_save6;
reg signed[15:0] data_save7;
reg signed[15:0] data_save8;


reg signed[15:0]   mul_a,mul_b;
wire signed[31:0]   mul_ab;


reg signed[23:0]   add_out;


assign fir_filter_ack_o = ( fir_filter_step_cnt == 'd9) ? 1'b1 : 1'b0;

always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0) begin
        data_save0  <= 'd0;
        data_save1  <= 'd0;
        data_save2  <= 'd0;
        data_save3  <= 'd0;
        data_save4  <= 'd0;
        data_save5  <= 'd0;
        data_save6  <= 'd0;
        data_save7  <= 'd0;
        data_save8  <= 'd0;
    end
    else if( fir_filter_req_i == 1'b1 && fir_filter_en== 1'b0) begin
        data_save0  <= data_i;
        data_save1  <= data_save0;
        data_save2  <= data_save1;
        data_save3  <= data_save2;
        data_save4  <= data_save3;
        data_save5  <= data_save4;
        data_save6  <= data_save5;
        data_save7  <= data_save6;
        data_save8  <= data_save7;

    end
    else begin
        data_save0  <= data_save0;
        data_save1  <= data_save1;
        data_save2  <= data_save2;
        data_save3  <= data_save3;
        data_save4  <= data_save4;
        data_save5  <= data_save5;
        data_save6  <= data_save6;
        data_save7  <= data_save7;
        data_save8  <= data_save8;

    end
end


always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0 )
        fir_filter_en <= 1'b0;
    else if( fir_filter_ack_o == 1'b1)
        fir_filter_en <= 1'b0;
    else if( fir_filter_req_i == 1'b1)
        fir_filter_en <= 1'b1;
    else
        fir_filter_en <= fir_filter_en;
end


always@(posedge sys_clk_i or negedge sys_rst_n_i )begin
    if( sys_rst_n_i == 1'b0)
        fir_filter_step_cnt <= 'd0;
    else if( fir_filter_en == 1'b1)
        fir_filter_step_cnt <= fir_filter_step_cnt + 1'b1;
    else
        fir_filter_step_cnt <= 'd0;

end



always@(posedge sys_clk_i) begin
    case(fir_filter_step_cnt)
    'd0: begin mul_a <= COE_0 ; mul_b <= data_save0; end
    'd1: begin mul_a <= COE_1 ; mul_b <= data_save1; end
    'd2: begin mul_a <= COE_2 ; mul_b <= data_save2; end
    'd3: begin mul_a <= COE_3 ; mul_b <= data_save3; end
    'd4: begin mul_a <= COE_4 ; mul_b <= data_save4; end
    'd5: begin mul_a <= COE_5 ; mul_b <= data_save5; end
    'd6: begin mul_a <= COE_6 ; mul_b <= data_save6; end
    'd7: begin mul_a <= COE_7 ; mul_b <= data_save7; end
    'd8: begin mul_a <= COE_8 ; mul_b <= data_save8; end
    default: begin mul_a <= COE_0; mul_b <= data_save0;end
    endcase
end

always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0) 
        add_out <= 'd0;
    else if( fir_filter_en == 1'b1)
        if( fir_filter_step_cnt > 'd0)
            add_out <= mul_ab + add_out;
        else
            add_out <= 'd0;
    else
        add_out <= 'd0;
end


always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0)
        filter_data_o <= 'd0;
    else if( fir_filter_step_cnt == 'd9)
        filter_data_o <= add_out >>> 8;
    else
        filter_data_o <= filter_data_o;
end

//由于数据只有16位有方向，而DSP为18*18，所以需要补充高位，防止方向改变
wire signed[35:0]   mul_c;
assign mul_ab=mul_c[31:0];
dsp_v1 dsp_v1_1(
    .clk	(sys_clk_i),
    .rstn	(sys_rst_n_i),
    .x0		({mul_a[15],mul_a[15],mul_a}),
    .y0		({mul_b[15],mul_b[15],mul_b}),
    .r0		(mul_c)
);

endmodule

在调用DSP的时候，需要注意不要寄存打拍数据，不然还要做数据同步，有点麻烦。
在这里插入图片描述
由于数据只有16位有方向，而DSP为18*18，所以需要补充高位，防止方向改变

reg signed[15:0]    mul_a,mul_b;
wire signed[31:0]   mul_ab;
wire signed[35:0]   mul_c;
assign mul_ab=mul_c[31:0];
dsp_v1 dsp_v1_1(
    .clk	(sys_clk_i),
    .rstn	(sys_rst_n_i),
    .x0		({mul_a[15],mul_a[15],mul_a}),
    .y0		({mul_b[15],mul_b[15],mul_b}),
    .r0		(mul_c)
);

iir滤波：角速度
角速度通常包含较高频率的变化，因为陀螺仪可以感知快速的旋转。IIR滤波器在处理高频信号时可能更为适用，因为它可以提供较好的动态响应。

`timescale 1ns / 1ps
//**************************************** Message ***********************************//
//技术交流：bumianzhe@126.com
//关注CSDN博主：“千歌叹尽执夏”
//Author: FPGA之旅 & 千歌叹尽执夏 
//All rights reserved	                               
//----------------------------------------------------------------------------------------
// Target Devices: 		H7P20N0L176-M2H1
// Tool Versions:       Fuxi 2023.1
// File name:           MPU6050_iir_filter
// Last modified Date:  2023年12月2日23:06:00
// Last Version:        V1.1
// Descriptions:        iir滤波器
//----------------------------------------------------------------------------------------
//****************************************************************************************//

module MPU6050_iir_filter(
    input                   sys_clk_i                   ,   //系统时钟
    input                   sys_rst_n_i                 ,   //系统复位

    input                   iir_filter_req_i            ,   //fir滤波请求
    input                   iir_filter_ack_o            ,   //fir滤波响应

    input signed[15:0]      data_i                      ,   //输入待滤波数据
    output reg signed[15:0] filter_data_o                   //滤波后的数据
);

reg iir_filter_en;
reg[4:0] iir_filter_step_cnt;
//滤波器系数  A对应输出   B对应输入   系数扩大了255
localparam COE_A0   =   $signed(255);
localparam COE_A1   =   $signed(191);  //取反 ，为了让后面的减法 变成 加法
localparam COE_A2   =   $signed(-69);  //取反

localparam COE_B0   =   $signed(33);
localparam COE_B1   =   $signed(67);
localparam COE_B2   =   $signed(33);



reg signed[15:0]    input_data_d0;
reg signed[15:0]    input_data_d1;
reg signed[15:0]    input_data_d2;


reg signed[15:0]    output_data_d0;
reg signed[15:0]    output_data_d1;



reg signed[15:0]    mul_a,mul_b;
wire signed[31:0]   mul_ab;

reg signed[31:0]    add_out;

assign iir_filter_ack_o = ( iir_filter_step_cnt == 'd5) ? 1'b1 : 1'b0;

always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0)
        iir_filter_en <= 1'b0;
    else if( iir_filter_ack_o == 1'b1)
        iir_filter_en <= 1'b0;
    else if( iir_filter_req_i == 1'b1 && iir_filter_en == 1'b0)
        iir_filter_en <= 1'b1;
    else
        iir_filter_en <= iir_filter_en;
end


always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0)
        iir_filter_step_cnt <= 'd0;
    else if( iir_filter_en == 1'b1)
        iir_filter_step_cnt <= iir_filter_step_cnt + 1'b1;
    else
        iir_filter_step_cnt <= 'd0; 

end


always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0) begin
        input_data_d0 <= 'd0;
        input_data_d1 <= 'd0;
        input_data_d2 <= 'd0;
    end
    else if( iir_filter_ack_o == 1'b1) begin
        input_data_d0 <= input_data_d0;
        input_data_d1 <= input_data_d0;
        input_data_d2 <= input_data_d1;
    end
    else if( iir_filter_req_i == 1'b1 && iir_filter_en == 1'b0) begin
        input_data_d0 <= data_i;
        input_data_d1 <= input_data_d1;
        input_data_d2 <= input_data_d2;
    end
    else begin
        input_data_d0 <= input_data_d0;
        input_data_d1 <= input_data_d1;
        input_data_d2 <= input_data_d2;
    end
end


always@(posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0) begin
        output_data_d0 <= 'd0;
        output_data_d1 <= 'd0;
    end
    else if( iir_filter_step_cnt == 'd5) begin
        output_data_d0 <= add_out >>> 8;
        output_data_d1 <= output_data_d0;
    end
    else begin
        output_data_d0 <= output_data_d0;
        output_data_d1 <= output_data_d1;
    end

end



always@(posedge sys_clk_i ) begin
    case(iir_filter_step_cnt)
    'd0: begin mul_a <= COE_B2 ; mul_b <= input_data_d2; end
    'd1: begin mul_a <= COE_B1 ; mul_b <= input_data_d1; end
    'd2: begin mul_a <= COE_B0 ; mul_b <= input_data_d0; end
    'd3: begin mul_a <= COE_A1 ; mul_b <= output_data_d0; end
    'd4: begin mul_a <= COE_A2 ; mul_b <= output_data_d1; end
    default:  begin mul_a <= COE_A0 ; mul_b <= input_data_d2; end
    endcase
end

always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0) 
        add_out <= 'd0;
    else if( iir_filter_en == 1'b1)
        if( iir_filter_step_cnt > 'd0)
            add_out <= mul_ab + add_out;
        else
            add_out <= 'd0;
    else
        add_out <= 'd0;
end

always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0)
        filter_data_o <= 'd0;
    else if( iir_filter_step_cnt == 'd5)
        filter_data_o <= (add_out >>> 8 ) ;//+ $signed(3);
    else
        filter_data_o <= filter_data_o;
end

//由于数据只有16位有方向，而DSP为18*18，所以需要补充高位，防止方向改变
wire signed[35:0]   mul_c;
assign mul_ab=mul_c[31:0];
dsp_v1 dsp_v1_1(
    .clk	(sys_clk_i),
    .rstn	(sys_rst_n_i),
    .x0		({mul_a[15],mul_a[15],mul_a}),
    .y0		({mul_b[15],mul_b[15],mul_b}),
    .r0		(mul_c)
);


endmodule

4、姿态解算

调用2个Cordic算法模块，对滤波后的数据进行解算，并输出计算后的数据。第一个Cordic算法模块先计算出roll和sqrt(acc_yacc_y + acc_z * acc_z)的值，然后第二个模块通过acc_x和sqrt(acc_yacc_y + acc_z * acc_z)的值计算出 pitch的角度。最后对融合后的数据进行一个打拍滤波。

`timescale 1ns / 1ps
//**************************************** Message ***********************************//
//技术交流：bumianzhe@126.com
//关注CSDN博主：“千歌叹尽执夏”
//Author: FPGA之旅 & 千歌叹尽执夏 
//All rights reserved	                               
//----------------------------------------------------------------------------------------
// Target Devices: 		H7P20N0L176-M2H1
// Tool Versions:       Fuxi 2023.1
// File name:           MPU6050_imu
// Last modified Date:  2023年8月26日12:00:00
// Last Version:        V1.1
// Descriptions:        MPU6050姿态解算
//----------------------------------------------------------------------------------------
//****************************************************************************************//

module MPU6050_imu(
    input                   sys_clk_i                   ,   //系统时钟
    input                   sys_rst_n_i                 ,   //系统复位

    input                   mpu6050_imu_req_i           ,
    output                  mpu6050_imu_ack_o           ,

    //输入数据
    input signed[15:0]      GYROX_i                     ,
    input signed[15:0]      GYROY_i                     ,
    input signed[15:0]      GYROZ_i                     ,
    input signed[15:0]      ACCELX_i                    ,
    input signed[15:0]      ACCELY_i                    ,
    input signed[15:0]      ACCELZ_i                    ,


    output signed[31:0]     mpu6050_angle_roll_o        ,   //三轴角度输出
    output signed[31:0]     mpu6050_angle_pitch_o       ,   //三轴角度输出
    output signed[31:0]     mpu6050_angle_yaw_o             //三轴角度输出
);

localparam  S_IDLE      =   'd0;
localparam  S_ROLL      =   'd1;
localparam  S_PITCH     =   'd2;
localparam  S_GYRO_RP   =   'd3;
localparam  S_IMU       =   'd4;
localparam  S_MEAN      =   'd5;
localparam  S_ACK       =   'd6;

reg[5:0]    state , next_state;



wire roll_req;
wire roll_ack;
wire signed[15:0]   amplitude_accy_accz;
wire signed[31:0]   acc_roll;

wire pitch_req;
wire pitch_ack;
wire signed[31:0]   acc_pitch;


//角速度单位时间内 造成的角度变化量 ( ( x / 4.1 ) / 100 ) <<< 16   ==  /512 + /2048  运算周期为 10ms
reg signed[31:0]    gyro_dt_roll;
reg signed[31:0]    gyro_dt_pitch;
reg signed[31:0]    gyro_dt_yaw;

reg signed[31:0]    gyro_roll;
reg signed[31:0]    gyro_pitch;
reg signed[31:0]    gyro_yaw;


//平均滤波
reg signed[31:0]    roll_d0;
reg signed[31:0]    roll_d1;
reg signed[31:0]    roll_d2;

reg signed[31:0]    pitch_d0;
reg signed[31:0]    pitch_d1;
reg signed[31:0]    pitch_d2;

reg signed[31:0]    yaw_d0;
reg signed[31:0]    yaw_d1;
reg signed[31:0]    yaw_d2;


//最终的角度值输出
reg signed[31:0] roll;
reg signed[31:0] pitch;
reg signed[31:0] yaw;

assign roll_req = ( state == S_ROLL) ? 1'b1 : 1'b0;
assign pitch_req = ( state == S_PITCH) ? 1'b1 : 1'b0;



assign mpu6050_imu_ack_o = ( state == S_ACK) ? 1'b1 : 1'b0;

assign mpu6050_angle_roll_o  = roll;
assign mpu6050_angle_pitch_o = pitch;
assign mpu6050_angle_yaw_o   = yaw;

always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0 )
        state <= S_IDLE;
    else 
        state <= next_state;
end

always@(*) begin
    case(state)
    S_IDLE:
        if( mpu6050_imu_req_i == 1'b1 )
            next_state <= S_ROLL;
        else
            next_state <= S_IDLE;
    S_ROLL:
        if( roll_ack == 1'b1)
            next_state <= S_PITCH;
        else
            next_state <= S_ROLL;
    S_PITCH:
        if( pitch_ack == 1'b1)
            next_state <= S_GYRO_RP;
        else
            next_state <= S_PITCH;
    S_GYRO_RP:
        next_state <= S_IMU;
    S_IMU:
        next_state <= S_MEAN;
    S_MEAN:
        next_state <= S_ACK;
    S_ACK:
        next_state <= S_IDLE;
    default: next_state <= S_IDLE;
    endcase

end






//计算gyro造成的角度变化量
always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0) begin
        gyro_dt_roll  <= 'd0;
        gyro_dt_pitch <= 'd0;
        gyro_dt_yaw   <= 'd0;
    end
    else if( state == S_ROLL && roll_ack == 1'b1) begin
        gyro_dt_roll  <= GYROX_i <<< 16;
        gyro_dt_pitch <= GYROY_i <<< 16;
        gyro_dt_yaw   <= GYROZ_i <<< 16;
    end
    else if( state == S_PITCH && pitch_ack == 1'b1) begin
        gyro_dt_roll  <= (gyro_dt_roll >>> 9) + (gyro_dt_roll >>> 11); 
        gyro_dt_pitch <= (gyro_dt_pitch >>> 9) + (gyro_dt_pitch >>> 11); 
        gyro_dt_yaw   <= (gyro_dt_yaw >>> 9) + (gyro_dt_yaw >>> 11); 

    end
    else begin
        gyro_dt_roll  <= gyro_dt_roll;
        gyro_dt_pitch <= gyro_dt_pitch;
        gyro_dt_yaw   <= gyro_dt_yaw;
    end
end



//计算角速度 测得的角度值
always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0) begin
        gyro_roll  <= 'd0;
        gyro_pitch <= 'd0;
        gyro_yaw   <= 'd0;
    end
    else if( state == S_GYRO_RP) begin
        gyro_roll  <= roll + gyro_dt_roll;
        gyro_pitch <= pitch - gyro_dt_pitch;
        gyro_yaw   <= yaw + gyro_dt_yaw;
    end
    else begin
        gyro_roll  <= gyro_roll;
        gyro_pitch <= gyro_pitch;
        gyro_yaw   <= gyro_yaw;
    end
end


//角度融合
always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0) begin
        roll  <= 'd0;
        pitch <= 'd0;
        yaw   <= 'd0;
    end
    else if( state == S_IMU) begin
        roll  <= (acc_roll >>> 1)  + (acc_roll >>> 2) + (gyro_roll >>> 2);
        pitch <= (acc_pitch >>> 1) + (acc_pitch >>> 2) + (gyro_pitch >>>2);
        yaw   <= gyro_yaw;
    end
    else if( state == S_MEAN) begin
        roll  <= (roll + roll_d0 ) >>> 1;
        pitch <= (pitch + pitch_d0 ) >>> 1;
        yaw <= (yaw + yaw_d0 ) >>> 1;
    end
    else begin
        roll  <= roll;
        pitch <= pitch;
        yaw   <= yaw;
    end

end
 

//对融合后的角度进行平均滤波
always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0) begin
        roll_d0 <= 'd0;
        roll_d1 <= 'd0;
        roll_d2 <= 'd0;
    end
    else if( state == S_ACK) begin
        roll_d0 <= roll;
        roll_d1 <= roll_d0;
        roll_d2 <= roll_d1;
    end
    else begin
        roll_d0 <= roll_d0;
        roll_d1 <= roll_d1;
        roll_d2 <= roll_d2;
    end
end 

always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0) begin
        pitch_d0 <= 'd0;
        pitch_d1 <= 'd0;
        pitch_d2 <= 'd0;
    end
    else if( state == S_ACK) begin
        pitch_d0 <= pitch;
        pitch_d1 <= pitch_d0;
        pitch_d2 <= pitch_d1;
    end
    else begin
        pitch_d0 <= pitch_d0;
        pitch_d1 <= pitch_d1;
        pitch_d2 <= pitch_d2;
    end
end 

always@(posedge sys_clk_i or negedge sys_rst_n_i) begin
    if( sys_rst_n_i == 1'b0) begin
        yaw_d0 <= 'd0;
        yaw_d1 <= 'd0;
        yaw_d2 <= 'd0;
    end
    else if( state == S_ACK) begin
        yaw_d0 <= yaw;
        yaw_d1 <= yaw_d0;
        yaw_d2 <= yaw_d1;
    end
    else begin
        yaw_d0 <= yaw_d0;
        yaw_d1 <= yaw_d1;
        yaw_d2 <= yaw_d2;
    end
end 
Cordic_arctan  u0_Cordic_arctan(

    .clk                    (       sys_clk_i           ),
    .rst_n                  (       sys_rst_n_i         ),

    .cordic_req             (       roll_req            ),
    .cordic_ack             (       roll_ack            ),

    .X                      (       ACCELZ_i            ),
    .Y                      (       ACCELY_i            ),

    .amplitude              (       amplitude_accy_accz ),  //幅度，偏大1.64倍，这里做了近似处理
    .theta                  (       acc_roll            )//扩大了2^16
);

Cordic_arctan  u1_Cordic_arctan(

    .clk                    (       sys_clk_i           ),
    .rst_n                  (       sys_rst_n_i         ),

    .cordic_req             (       pitch_req           ),
    .cordic_ack             (       pitch_ack           ),

    .X                      (       amplitude_accy_accz ),
    .Y                      (       ACCELX_i            ),

    .amplitude              (                           ),  //幅度，偏大1.64倍，这里做了近似处理
    .theta                  (       acc_pitch           )//扩大了2^16
);

endmodule

六、资源消耗&工程获取

因为代码写的比较冗余，暂时还没有优化，所以暂用了较多的寄存器。
在这里插入图片描述
链接：https://pan.baidu.com/s/13fpE8ncS_-kW4XjDe2Xpmw?pwd=JWQL
提取码：JWQL
–来自百度网盘超级会员V6的分享

七、总结

      MPU6050具有高精度、低功耗和成本低廉的优点。它可以实现较高的测量精度和稳定性，同时功耗较低。另外，由于其成本低廉，可以广泛应用于各种领域。
      然而，MPU6050也存在一些缺点。首先，它只能测量物体在x、y、z三个方向上的加速度和角速度，无法对物体的位置进行直接测量。其次，MPU6050的测量结果受到环境因素的影响较大，需要进行校准以保证测量精度。最后，由于其测量结果的准确性受到多种因素的影响，需要通过算法进行数据处理和滤波，才能得到可靠的结果。
      总之，MPU6050可以作为简单版平衡车一类作品的姿态获取，如果作为无人机等精度较高的，还需要使用九轴传感器并结合高阶滤波器的信息作为校准点。