京微齐力：基于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 ***********************************//
//技术交流：[email protected]
//关注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 ***********************************//
//技术交流：[email protected]
//关注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 ***********************************//
//技术交流：[email protected]
//关注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 ***********************************//
//技术交流：[email protected]
//关注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 ***********************************//
//技术交流：[email protected]
//关注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可以作为简单版平衡车一类作品的姿态获取，如果作为无人机等精度较高的，还需要使用九轴传感器并结合高阶滤波器的信息作为校准点。