本文主要是介绍京微齐力：基于H7的平衡控制系统（一、姿态解析），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
目录
- 前言
- 一、关于平衡控制系统
- 二、实验效果
- 三、硬件选择
- 1、H7P20N0L176-M2H1
- 2、MPU6050
- 四、理论简述
- 五、程序设计
- 1、Cordic算法
- 2、MPU6050采集数据
- 3、fir&iir滤波
- 4、姿态解算
- 六、资源消耗&工程获取
- 七、总结
前言
很久之前，就想用纯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^16reg 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^16always@(posedge clk)
beginrot[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;
endalways@(posedge clk or negedge rst_n)
beginif( rst_n == 1'b0)cal_delay[0] <= 1'b0;elsecal_delay[0] <= cordic_req;
endgenvar j;
generatefor(j = 1 ;j < 17 ; j = j + 1)begin: loopalways@(posedge clk or negedge rst_n)beginif( rst_n == 1'b0)cal_delay[j] <= 1'b0;elsecal_delay[j] <= cal_delay[j-1];endend
endgenerate//将坐标挪到第一和四项限中
always@(posedge clk or negedge rst_n)
beginif( rst_n == 1'b0)beginXn[0] <= 'd0;Yn[0] <= 'd0;Zn[0] <= 'd0;endelse if( cordic_req == 1'b1)beginif( X < $signed(0) && Y < $signed(0))beginXn[0] <= -(X << 16);Yn[0] <= -(Y << 16);endelse if( X < $signed(0) && Y > $signed(0))beginXn[0] <= -(X << 16);Yn[0] <= -(Y << 16);endelsebeginXn[0] <= X << 16;Yn[0] <= Y << 16;endZn[0] <= 'd0;endelse beginXn[0] <= Xn[0];Yn[0] <= Yn[0];Zn[0] <= Zn[0];end
end//旋转
genvar i;
generatefor( i = 1 ;i < 17 ;i = i+1)begin: loop2always@(posedge clk or negedge rst_n)beginif( rst_n == 1'b0)beginXn[i] <= 'd0;Yn[i] <= 'd0;Zn[i] <= 'd0;endelse if( cal_delay[i -1] == 1'b1)beginif( Yn[i-1][31] == 1'b0)beginXn[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];endelsebeginXn[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];endendelsebeginXn[i] <= Xn[i];Yn[i] <= Yn[i];Zn[i] <= Zn[i];endendend
endgenerateendmodule2、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，为了保留精度，指除以256assign 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) beginif( rst_n == 1'b0)state <= S_IDLE;elsestate <= next_state;
endalways@(*)begincase(state)S_IDLE:if( mpu6050_req == 1'b1 )next_state <= S_READ_GYRO;elsenext_state <= S_IDLE;S_READ_GYRO:if( GYRODone == 1'b1)next_state <= S_READ_ACCEL;elsenext_state <= S_READ_GYRO;S_READ_ACCEL:if( ACCELDone == 1'b1)next_state <= S_ACK;elsenext_state <= S_READ_ACCEL;S_ACK:next_state <= S_IDLE;default: next_state <= S_IDLE;endcase
endMPU6050_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 )
);endmodule3、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) beginif( sys_rst_n_i == 1'b0) begindata_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;endelse if( fir_filter_req_i == 1'b1 && fir_filter_en== 1'b0) begindata_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;endelse begindata_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
endalways@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( 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;elsefir_filter_en <= fir_filter_en;
endalways@(posedge sys_clk_i or negedge sys_rst_n_i )beginif( 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;elsefir_filter_step_cnt <= 'd0;endalways@(posedge sys_clk_i) begincase(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; enddefault: begin mul_a <= COE_0; mul_b <= data_save0;endendcase
endalways@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( 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;elseadd_out <= 'd0;elseadd_out <= 'd0;
endalways@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( sys_rst_n_i == 1'b0)filter_data_o <= 'd0;else if( fir_filter_step_cnt == 'd9)filter_data_o <= add_out >>> 8;elsefilter_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) beginif( 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;elseiir_filter_en <= iir_filter_en;
endalways@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( 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;elseiir_filter_step_cnt <= 'd0; endalways@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( sys_rst_n_i == 1'b0) begininput_data_d0 <= 'd0;input_data_d1 <= 'd0;input_data_d2 <= 'd0;endelse if( iir_filter_ack_o == 1'b1) begininput_data_d0 <= input_data_d0;input_data_d1 <= input_data_d0;input_data_d2 <= input_data_d1;endelse if( iir_filter_req_i == 1'b1 && iir_filter_en == 1'b0) begininput_data_d0 <= data_i;input_data_d1 <= input_data_d1;input_data_d2 <= input_data_d2;endelse begininput_data_d0 <= input_data_d0;input_data_d1 <= input_data_d1;input_data_d2 <= input_data_d2;end
endalways@(posedge sys_clk_i or negedge sys_rst_n_i ) beginif( sys_rst_n_i == 1'b0) beginoutput_data_d0 <= 'd0;output_data_d1 <= 'd0;endelse if( iir_filter_step_cnt == 'd5) beginoutput_data_d0 <= add_out >>> 8;output_data_d1 <= output_data_d0;endelse beginoutput_data_d0 <= output_data_d0;output_data_d1 <= output_data_d1;endendalways@(posedge sys_clk_i ) begincase(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; enddefault: begin mul_a <= COE_A0 ; mul_b <= input_data_d2; endendcase
endalways@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( 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;elseadd_out <= 'd0;elseadd_out <= 'd0;
endalways@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( 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);elsefilter_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) beginif( sys_rst_n_i == 1'b0 )state <= S_IDLE;else state <= next_state;
endalways@(*) begincase(state)S_IDLE:if( mpu6050_imu_req_i == 1'b1 )next_state <= S_ROLL;elsenext_state <= S_IDLE;S_ROLL:if( roll_ack == 1'b1)next_state <= S_PITCH;elsenext_state <= S_ROLL;S_PITCH:if( pitch_ack == 1'b1)next_state <= S_GYRO_RP;elsenext_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;endcaseend//计算gyro造成的角度变化量
always@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( sys_rst_n_i == 1'b0) begingyro_dt_roll <= 'd0;gyro_dt_pitch <= 'd0;gyro_dt_yaw <= 'd0;endelse if( state == S_ROLL && roll_ack == 1'b1) begingyro_dt_roll <= GYROX_i <<< 16;gyro_dt_pitch <= GYROY_i <<< 16;gyro_dt_yaw <= GYROZ_i <<< 16;endelse if( state == S_PITCH && pitch_ack == 1'b1) begingyro_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); endelse begingyro_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) beginif( sys_rst_n_i == 1'b0) begingyro_roll <= 'd0;gyro_pitch <= 'd0;gyro_yaw <= 'd0;endelse if( state == S_GYRO_RP) begingyro_roll <= roll + gyro_dt_roll;gyro_pitch <= pitch - gyro_dt_pitch;gyro_yaw <= yaw + gyro_dt_yaw;endelse begingyro_roll <= gyro_roll;gyro_pitch <= gyro_pitch;gyro_yaw <= gyro_yaw;end
end//角度融合
always@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( sys_rst_n_i == 1'b0) beginroll <= 'd0;pitch <= 'd0;yaw <= 'd0;endelse if( state == S_IMU) beginroll <= (acc_roll >>> 1) + (acc_roll >>> 2) + (gyro_roll >>> 2);pitch <= (acc_pitch >>> 1) + (acc_pitch >>> 2) + (gyro_pitch >>>2);yaw <= gyro_yaw;endelse if( state == S_MEAN) beginroll <= (roll + roll_d0 ) >>> 1;pitch <= (pitch + pitch_d0 ) >>> 1;yaw <= (yaw + yaw_d0 ) >>> 1;endelse beginroll <= roll;pitch <= pitch;yaw <= yaw;endend//对融合后的角度进行平均滤波
always@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( sys_rst_n_i == 1'b0) beginroll_d0 <= 'd0;roll_d1 <= 'd0;roll_d2 <= 'd0;endelse if( state == S_ACK) beginroll_d0 <= roll;roll_d1 <= roll_d0;roll_d2 <= roll_d1;endelse beginroll_d0 <= roll_d0;roll_d1 <= roll_d1;roll_d2 <= roll_d2;end
end always@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( sys_rst_n_i == 1'b0) beginpitch_d0 <= 'd0;pitch_d1 <= 'd0;pitch_d2 <= 'd0;endelse if( state == S_ACK) beginpitch_d0 <= pitch;pitch_d1 <= pitch_d0;pitch_d2 <= pitch_d1;endelse beginpitch_d0 <= pitch_d0;pitch_d1 <= pitch_d1;pitch_d2 <= pitch_d2;end
end always@(posedge sys_clk_i or negedge sys_rst_n_i) beginif( sys_rst_n_i == 1'b0) beginyaw_d0 <= 'd0;yaw_d1 <= 'd0;yaw_d2 <= 'd0;endelse if( state == S_ACK) beginyaw_d0 <= yaw;yaw_d1 <= yaw_d0;yaw_d2 <= yaw_d1;endelse beginyaw_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可以作为简单版平衡车一类作品的姿态获取，如果作为无人机等精度较高的，还需要使用九轴传感器并结合高阶滤波器的信息作为校准点。
这篇关于京微齐力：基于H7的平衡控制系统（一、姿态解析）的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
