利用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_y*acc_y + acc_z * acc_z)的值，然后第二个模块通过acc_x和sqrt(acc_y*acc_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; endcaseend
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 ));
模块已上板测试，解算出来的角度没有问题(可能精度不是那么完美，滤波与融合那里需要下点功夫)。
利用DS18B20做一个温控器（DS18B20引脚图_工作原理及应用电路）
关于光通信的最强进阶科普
联想正式开始输出AI能力 LeapAI平台的独特性保证了企业的竞争力
国产无人机引领全球发展热潮 行业标准仍待规范
全球3G网络选择：170国WCDMA 129国CDMA200
利用Cordic算法来进行姿态解算
5G终端连接数已超3600万，四大运营商发布最新5G发展目标
2020年世界人工智能大会云端峰会申城召开 白玉兰开源联盟诞生
iPhone4S手机芯片级拆解
燃料电池VS锂电池 性能孰优孰劣？
芯片滤波器设计实战指南
五款经典运放电路分享
商务部公布今年双11全网销售额 相比阿里多近600个亿
罗德与施瓦茨FPH手持式频谱分析仪提供44GHz测量频率
10 个增加 UNIX/Linux Shell 脚本趣味的工具
uboot图形化配置及其原理
整流电路的作用
首款支持最新蓝牙5.0功能的磁带录音机问世
3D视觉传感器——包装和物流案例分析（机器人拾取放）
学生党蓝牙耳机，千元内性价比蓝牙耳机推荐
在进行姿态解算分享之前，先分享一个踩坑经历。一般来说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_y*acc_y + acc_z * acc_z)的值，然后第二个模块通过acc_x和sqrt(acc_y*acc_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; endcaseend
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 ));
模块已上板测试，解算出来的角度没有问题(可能精度不是那么完美，滤波与融合那里需要下点功夫)。
利用DS18B20做一个温控器（DS18B20引脚图_工作原理及应用电路）
关于光通信的最强进阶科普
联想正式开始输出AI能力 LeapAI平台的独特性保证了企业的竞争力
国产无人机引领全球发展热潮 行业标准仍待规范
全球3G网络选择：170国WCDMA 129国CDMA200
利用Cordic算法来进行姿态解算
5G终端连接数已超3600万，四大运营商发布最新5G发展目标
2020年世界人工智能大会云端峰会申城召开 白玉兰开源联盟诞生
iPhone4S手机芯片级拆解
燃料电池VS锂电池 性能孰优孰劣？
芯片滤波器设计实战指南
五款经典运放电路分享
商务部公布今年双11全网销售额 相比阿里多近600个亿
罗德与施瓦茨FPH手持式频谱分析仪提供44GHz测量频率
10 个增加 UNIX/Linux Shell 脚本趣味的工具
uboot图形化配置及其原理
整流电路的作用
首款支持最新蓝牙5.0功能的磁带录音机问世
3D视觉传感器——包装和物流案例分析（机器人拾取放）
学生党蓝牙耳机，千元内性价比蓝牙耳机推荐