代码解读一 文件名“ANO_Imu.c”
2024-06-09 06:22:38
我把这个文件的所有代码贴上来了，供大家参考，由于本人水平有限，且匿名代码注释比较少，所以很多也不是很懂，实在是一些莫名的定义太多了，什么w/x/y/z/h之类的，只求先能看懂大概逻辑，至于一些细节日后再啃
先来一个融合磁力计的Mahony互补滤波算法
https://blog.csdn.net/zhiyu_buliang/article/details/89056519
有心的人会发现匿名现在的代码也不过是根据原来经典Mahony代码改过来的，大致意思都在，仔细对比下看看。
本人在匿名代码的基础上增加了一些注释，只是个人的见解，免不了会有很多错误，希望大家多多指正。
/******************** (C) COPYRIGHT 2016 ANO Tech **************************** 作者 ：匿名科创* 文件名 ：ANO_IMU.c* 描述 ：姿态解算函数* 官网 ：www.anotc.com* 淘宝 ：anotc.taobao.com* 技术Q群 ：190169595
*****************************************************************************/
#include "Ano_Imu.h"
#include "Ano_Math.h"
#include "Ano_Filter.h"
#include "Ano_DT.h"
//#include "ANO_RC.h"/*参考坐标，定义为ANO坐标 ---> 西北天俯视，机头方向为x正方向+x|+y--|--|*/ //涉及磁力计的XY二维变换
//原谅我没看懂到底是干嘛的
void w2h_2d_trans(float w[VEC_XYZ],float ref_ax[VEC_XYZ],float h[VEC_XYZ])
{h[X] = w[X] * ref_ax[X] + w[Y] *ref_ax[Y];h[Y] = w[X] *(-ref_ax[Y]) + w[Y] *ref_ax[X];}void h2w_2d_trans(float h[VEC_XYZ],float ref_ax[VEC_XYZ],float w[VEC_XYZ])
{w[X] = h[X] *ref_ax[X] + h[Y] *(-ref_ax[Y]);w[Y] = h[X] *ref_ax[Y] + h[Y] * ref_ax[X];}//没看懂
float mag_yaw_calculate(float dT,float mag_val[VEC_XYZ],float g_z_vec[VEC_XYZ],float h_mag_val[VEC_XYZ])//
{
// float mag_h_norm;
// float mag_2d_vec[VEC_XYZ];vec_3dh_transition(g_z_vec, mag_val, h_mag_val);// mag_h_norm = my_sqrt(my_pow(h_mag_val[X]) + my_pow(h_mag_val[Y]));
//
// mag_2d_vec[X] = safe_div(h_mag_val[X],mag_h_norm,0);
// mag_2d_vec[Y] = safe_div(h_mag_val[Y],mag_h_norm,0);// return (fast_atan2(mag_2d_vec[Y], mag_2d_vec[X]) *57.3f) ;// return (fast_atan2(h_mag_val[Y], h_mag_val[X]) *57.3f) ;//
}#define USE_MAG
#define USE_LENGTH_LIM//imu最长的结构体定义变量赋初值
_imu_st imu_data = {1,0,0,0,{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},0,0,0};//imu状态结构体定义变量赋初值
_imu_state_st imu_state = {1,1,1,1,1,1,1,1};static float vec_err[VEC_XYZ];
static float vec_err_i[VEC_XYZ];
static float q0q1,q0q2,q1q1,q1q3,q2q2,q2q3,q3q3,q1q2,q0q3;//q0q0,
static float mag_yaw_err,mag_val_f[VEC_XYZ];
static float imu_reset_val; static u16 reset_cnt;//这个t变量很重要啊
float t[3][3],t_temp;//float rpy_enr[VEC_XYZ],rpy_f1_a[VEC_XYZ],rpy_f1_b[VEC_XYZ],vec_err_f1[VEC_XYZ];
float imu_test[3];//重中之重 理解姿态解算全靠它了
//此函数在文件 Ano_FlightDataCal.c 被调用，
//入口参数 采样周期/imu状态结构体指针(实际传入的是&imu_state)/三向陀螺仪/三向加速度计/三向磁力计/imu结构体句柄指针(实际传入的是&imu_data)
void IMU_update(float dT,_imu_state_st *state,float gyr[VEC_XYZ], s32 acc[VEC_XYZ],s16 mag_val[VEC_XYZ],_imu_st *imu)
{
// const float kp = 0.2f,ki = 0.001f;
// const float kmp = 0.1f;static float kp_use = 0,ki_use = 0,mkp_use = 0;float acc_norm_l,acc_norm_l_recip,q_norm_l;float acc_norm[VEC_XYZ];float d_angle[VEC_XYZ];//先行计算好，直接调用速度快//其实w/x/y/z是和q0/1/2/3是一样的，可能指针运算速度快也比较方便
// q0q0 = imu->w * imu->w; q0q1 = imu->w * imu->x;q0q2 = imu->w * imu->y;q1q1 = imu->x * imu->x;q1q3 = imu->x * imu->z;q2q2 = imu->y * imu->y;q2q3 = imu->y * imu->z;q3q3 = imu->z * imu->z;q1q2 = imu->x * imu->y;q0q3 = imu->w * imu->z;//根据文件 Ano_FlightDataCal.c 不难看出obs_en始终为0if(state->obs_en){//计算机体坐标下的运动加速度观测量。坐标系为北西天for(u8 i = 0;i<3;i++){s32 temp = 0;for(u8 j = 0;j<3;j++){temp += imu->obs_acc_w[j] *t[j][i];//t[i][j] 转置为 t[j][i]}imu->obs_acc_a[i] = temp;imu->gra_acc[i] = acc[i] - imu->obs_acc_a[i];}}//只执行下面这句//把测量值传给指针保存，这种操作很常见else{for(u8 i = 0;i<3;i++){ imu->gra_acc[i] = acc[i];}}//根号分之一acc_norm_l_recip = my_sqrt_reciprocal(my_pow(imu->gra_acc[X]) + my_pow(imu->gra_acc[Y]) + my_pow(imu->gra_acc[Z]));//根号acc_norm_l = safe_div(1,acc_norm_l_recip,0);//加速度计的读数，单位化。for(u8 i = 0;i<3;i++){acc_norm[i] = imu->gra_acc[i] *acc_norm_l_recip;}// 载体坐标下的x方向向量，单位化。 RCb 与 (1 0 0)的转置相乘t[0][0] = imu->x_vec[X] = 1 - (2*q2q2 + 2*q3q3);t[0][1] = imu->x_vec[Y] = 2*q1q2 - 2*q0q3;t[0][2] = imu->x_vec[Z] = 2*q1q3 + 2*q0q2;// 载体坐标下的y方向向量，单位化。 RCb 与 (0 1 0)的转置相乘t[1][0] = imu->y_vec[X] = 2*q1q2 + 2*q0q3;t[1][1] = imu->y_vec[Y] = 1 - (2*q1q1 + 2*q3q3);t[1][2] = imu->y_vec[Z] = 2*q2q3 - 2*q0q1;// 载体坐标下的z方向向量（等效重力向量、重力加速度向量），单位化 RCb 与 (0 0 1)的转置相乘t[2][0] = imu->z_vec[X] = 2*q1q3 - 2*q0q2;t[2][1] = imu->z_vec[Y] = 2*q2q3 + 2*q0q1;t[2][2] = imu->z_vec[Z] = 1 - (2*q1q1 + 2*q2q2);//水平面方向向量imu->hx_vec[X] = t[0][0];imu->hx_vec[Y] = t[1][0];//没懂//计算载体坐标下的运动加速度。(与姿态解算无关)for(u8 i = 0;i<3;i++){imu->a_acc[i] = (s32)(acc[i] - 981 *imu->z_vec[i]);}//计算世界坐标下的运动加速度。坐标系为北西天for(u8 i = 0;i<3;i++){s32 temp = 0;for(u8 j = 0;j<3;j++){temp += imu->a_acc[j] *t[i][j];}imu->w_acc[i] = temp;}w2h_2d_trans(imu->w_acc,imu_data.hx_vec,imu->h_acc);//计算向量误差vec_err[X] = (acc_norm[Y] * imu->z_vec[Z] - imu->z_vec[Y] * acc_norm[Z]);vec_err[Y] = -(acc_norm[X] * imu->z_vec[Z] - imu->z_vec[X] * acc_norm[Z]);vec_err[Z] = -(acc_norm[Y] * imu->z_vec[X] - imu->z_vec[Y] * acc_norm[X]);#ifdef USE_MAG//计算航向yaw误差for(u8 i = 0;i<3;i++){mag_val_f[i] = (float)mag_val[i];} if(!(mag_val[X] ==0 && mag_val[Y] == 0 && mag_val[Z] == 0)){mag_yaw_err = mag_yaw_calculate(dT,mag_val_f,(imu->z_vec),(imu->h_mag)) - imu->yaw;mag_yaw_err = range_to_180deg(mag_yaw_err); }
#endiffor(u8 i = 0;i<3;i++){#ifdef USE_EST_DEADZONE if(state->G_reset == 0 && state->obs_en == 0){vec_err[i] = my_deadzone(vec_err[i],0,imu->gacc_deadzone[i]);}
#endif #ifdef USE_LENGTH_LIM if(acc_norm_l>1060 || acc_norm_l<900){vec_err[X] = vec_err[Y] = vec_err[Z] = 0;}
#endif//误差积分vec_err_i[i] += LIMIT(vec_err[i],-0.01f,0.01f) *dT *ki_use;// 构造增量旋转（含融合纠正）。 // d_angle[X] = (gyr[X] + (vec_err[X] + vec_err_i[X]) * kp_use - mag_yaw_err *imu->z_vec[X] *kmp_use *RAD_PER_DEG) * dT / 2 ;// d_angle[Y] = (gyr[Y] + (vec_err[Y] + vec_err_i[Y]) * kp_use - mag_yaw_err *imu->z_vec[Y] *kmp_use *RAD_PER_DEG) * dT / 2 ;// d_angle[Z] = (gyr[Z] + (vec_err[Z] + vec_err_i[Z]) * kp_use - mag_yaw_err *imu->z_vec[Z] *kmp_use *RAD_PER_DEG) * dT / 2 ;//PI补偿
#ifdef USE_MAGd_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use - mag_yaw_err *imu->z_vec[i] *mkp_use *RAD_PER_DEG) * dT / 2 ;
#elsed_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use ) * dT / 2 ;
#endif}//计算四元数q0/1/2/3imu->w = imu->w - imu->x*d_angle[X] - imu->y*d_angle[Y] - imu->z*d_angle[Z];imu->x = imu->w*d_angle[X] + imu->x + imu->y*d_angle[Z] - imu->z*d_angle[Y];imu->y = imu->w*d_angle[Y] - imu->x*d_angle[Z] + imu->y + imu->z*d_angle[X];imu->z = imu->w*d_angle[Z] + imu->x*d_angle[Y] - imu->y*d_angle[X] + imu->z;//四元数单位化q_norm_l = my_sqrt_reciprocal(imu->w*imu->w + imu->x*imu->x + imu->y*imu->y + imu->z*imu->z);imu->w *= q_norm_l;imu->x *= q_norm_l;imu->y *= q_norm_l;imu->z *= q_norm_l;//下面这些以及文件 Ano_FlightDataCal.c中的使用state结构体来通过设定gkp/gki/mkp三个系数进行//误差修正使误差限制在一个极小的范围内，如果超出范围则会继续修正，直至满足要求，简单就可理解为//PI补偿。同时还伴随有复位标记设置，可用于对IMU进行复位./修正开关///
#ifdef USE_MAGif(state->M_fix_en==0)//磁力{mkp_use = 0;//不修正state->M_reset = 0;//罗盘修正不复位，清除复位标记}else{if(state->M_reset)//{//通过增量进行对准mkp_use = 5.0f;if(mag_yaw_err != 0 && ABS(mag_yaw_err)<2){state->M_reset = 0;//误差小于2的时候，清除复位标记}}else{mkp_use = state->mkp; //正常修正}}
#endifif(state->G_fix_en==0)//重力方向修正{kp_use = 0;//不修正}else{if(state->G_reset == 0)//正常修正{ kp_use = state->gkp;ki_use = state->gki;}else//快速修正，通过增量进行对准{kp_use = 5.0f;ki_use = 0.5f;
// imu->est_speed_w[X] = imu->est_speed_w[Y] = 0;
// imu->est_acc_w[X] = imu->est_acc_w[Y] = 0;
// imu->est_acc_h[X] = imu->est_acc_h[Y] =0;//计算静态误差是否缩小imu_reset_val += (ABS(vec_err[X]) + ABS(vec_err[Y])) *1000 *dT;imu_reset_val -= 0.01f;imu_reset_val = LIMIT(imu_reset_val,0,1.0f);if((imu_reset_val < 0.01f) && (state->M_reset == 0)){//计时reset_cnt += 2;if(reset_cnt>500){reset_cnt = 0;state->G_reset = 0;//已经对准，清除复位标记}}else{reset_cnt = 0;}}}
}//计算欧拉角
//一般来说都是通过解四元数微分方程得出四元数然后通过反三角函数得出欧拉角
//但是这个欧拉角计算看着并不像
void calculate_RPY()
{///输出姿态角///t_temp = LIMIT(1 - my_pow(t[2][0]),0,1);//imu_data.pit = asin(2*q1q3 - 2*q0q2)*57.30f;if(ABS(imu_data.z_vec[Z])>0.05f)//避免奇点的运算{imu_data.pit = fast_atan2(t[2][0],my_sqrt(t_temp))*57.30f;imu_data.rol = fast_atan2(t[2][1], t[2][2])*57.30f; imu_data.yaw = -fast_atan2(t [1][0], t[0][0])*57.30f; }
}/******************* (C) COPYRIGHT 2016 ANO TECH *****END OF FILE************/对比不难发现
- 首先都是定义一些常用的变量，后面直接调用确实能加快速度
- 同样是归一化，只不过他的归一化比较骚操作而已
- 从n系变换到b系，为后面求误差做准备
- 利用理论与实际叉乘求误差，骚操作而已
- 进行PI补偿
- 解出四元数然后归一化最后不难得到欧拉角
不难看出，大体上两者是差不多的，匿名对经典代码进行了相当的优化，至于效果怎么样，就需要自己亲身试试。然后呢，姿态解算到这里就差不多先告一段落了，后面就该搞搞PID了，这个头疼的玩意，哎呀…
代码解读一 文件名“ANO_Imu.c”相关推荐
- 代码解读四 文件名“Ano_AttCtrl.c”
这部分是关于匿名串级PID的,我觉得有需要的同学可以直接移植,不需要自己写了,确实有点麻烦,基本上代码里面都注释的很清楚了,且由于本人水平有限,所以也不是都很懂,只能做到这里了. #include & ...
- 代码解读六 文件名“Ano_AltCtrl.c”
写了一大堆,也不知道对不对,贴上来让大家看看 #include "Ano_AltCtrl.h" //高度控制 #include "Ano_Imu.h" #inc ...
- 代码解读十 文件名“Ano_FlightDataCal.c”
本部分主要是对IMU测量模块测量的值进行后续处理,同时在飞行过程中不断对数据进行更新,然后进行姿态解算,便于后续丢进PID中进行进一步处理.根据所处位置及函数调用情况不难发现此部分算是对底层的进一步封 ...
- STM32学习心得二十一：实时时钟RTC和备份寄存器BKP特征、原理及相关实验代码解读
记录一下,方便以后翻阅~ 主要内容 1) RTC特征与原理: 2) BKP备份寄存器特征与原理: 3) RTC常用寄存器+库函数介绍: 4) 相关实验代码解读. 实验内容: 因为没有买LCD屏,所以计 ...
- 基于实例分割方法的端到端车道线检测 论文+代码解读
Towards End-to-End Lane Detection: an Instance Segmentation Approach 论文原文 https://arxiv.org/pdf/1802 ...
- Pytorch_DDC（深度网络自适应，以resnet50为例）代码解读
最近跑了一下王晋东博士迁移学习简明手册上的深度网络自适应DDC(Deep Domain Confusion)的代码实现,在这里做一下笔记. 来源:Githup开源链接 总结代码的大体框架如下: 1.数 ...
- 联邦学习开山之作代码解读与收获
参考:联邦学习代码解读,超详细_一只揪°的博客-CSDN博客_联邦学习代码 参考文献:[1602.05629] Communication-Efficient Learning of Deep Net ...
- BEGAN-边界均衡生成对抗网络-代码解读
当前论文代码 首先注意: 不同点: 该论文的输入是噪音,鉴别器和生成器都是哑铃型结构, 相同点: 输出是一张图片,D都是用真实图像去比对. 已知信息 可见,是从main.py开始训练的.测试的时候,只 ...
- 200行代码解读TDEngine背后的定时器
作者 | beyondma来源 | CSDN博客 导读:最近几周,本文作者几篇有关陶建辉老师最新的创业项目-TdEngine代码解读文章出人意料地引起了巨大的反响,原以为C语言已经是昨日黄花,不过从读 ...
最新文章
- TCP的三次握手和四次分手
- 英国首相将授权华为接入英国5G网络
- layui树形父子不关联_DP专题7 | 没有上司的舞会 洛谷1352（树形DP）
- AJAX是否能够取代桌面应用程序
- 六十一、Vue中父子组件传值和组件参数校验
- 40万总奖金！顶级云服务免费用！2021全球高性能云计算创新大赛报名中！
- 算术基本定理（维基百科）
- Main函数参数argc，argv如何传入
- java switch中if_详解java中if语句和switch的使用
- Google地球查看香港地形
- 大数据技术在各行业中的挑战有哪些
- 黑客语言Python
- 评分卡模型分数转换整个流程
- java rrd 读取_RRD插入值的计算方式
- 鸿蒙2.0 134个仓库扼要说明
- ERP系统物料清单设计技巧
- 土地资源管理本科毕业论文有哪些选题推荐？
- 2018.8.4T3（大容斥）
- Pulmonary--Detection7
- 如何利用开源插件？又快又好地搞好数据接口开发，连通不同应用系统