代码解读一 文件名“ANO_Imu.c”

我把这个文件的所有代码贴上来了，供大家参考，由于本人水平有限，且匿名代码注释比较少，所以很多也不是很懂，实在是一些莫名的定义太多了，什么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},