匿名姿态解算算法带注释

/******************** （C） COPYRIGHT 2014 ANO Tech ********************************
  * 作者   ：匿名科创
 * 文件名  ：imu.c
 * 描述    ：姿态解算
 * 官网    ：www.anotc.com
 * 淘宝    ：anotc.taobao.com
 * 技术Q群 ：190169595
**********************************************************************************/

#include “imu.h“
#include “include.h“
#include “ak8975.h“
#include “mymath.h“

#define Kp 0.6f                	// proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.1f                	// 0.001  integral gain governs rate of convergence of gyroscope biases

#define IMU_INTEGRAL_LIM  （ 2.0f *ANGLE_TO_RADIAN ）
#define NORM_ACC_LPF_HZ 10  		//（Hz）
#define REF_ERR_LPF_HZ  1				//（Hz）

xyz_f_t reference_v;
ref_t 	ref;

xyz_f_t Gravity_Vec;  				//解算的重力向量
	
float RollPitchYaw;    				//姿态角

float ref_q[4] = {1000};
float norm_accnorm_q;
float norm_acc_lpf;

extern u8 fly_ready;
void IMUupdate（float half_Tfloat gx float gy float gz float ax float ay float azfloat *rolfloat *pitfloat *yaw） 
{		
	float ref_err_lpf_hz;
	static float mag_norm mag_tmp_xmag_tmp_yyaw_mag;

	mag_norm = my_sqrt（ak8975.Mag_Val.x * ak8975.Mag_Val.x + ak8975.Mag_Val.y * ak8975.Mag_Val.y）;
	
	if（ak8975.Mag_Val.x != 0 && ak8975.Mag_Val.y != 0 ）
	{
		mag_tmp_x = （float）ak8975.Mag_Val.x /mag_norm;
		mag_tmp_y = （float）ak8975.Mag_Val.y /mag_norm;	
		yaw_mag = fast_atan2（mag_tmp_ymag_tmp_x） *57.3f;
	}
	//=============================================================================
	// 计算等效重力向量
	reference_v.x = 2*（ref_q[1]*ref_q[3] - ref_q[0]*ref_q[2]）;
	reference_v.y = 2*（ref_q[0]*ref_q[1] + ref_q[2]*ref_q[3]）;
	reference_v.z = 1 - 2*（ref_q[1]*ref_q[1] + ref_q[2]*ref_q[2]）;//ref_q[0]*ref_q[0] - ref_q[1]*ref_q[1] - ref_q[2]*ref_q[2] + ref_q[3]*ref_q[3];

	
	//这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
	//根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。
	//所以这里的vx\y\z，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的重力单位向量。       
	//=============================================================================


	// 计算加速度向量的模
	norm_acc = my_sqrt（ax*ax + ay*ay + az*az）;   
	norm_acc_lpf +=  NORM_ACC_LPF_HZ *（6.28f *half_T） *（norm_acc - norm_acc_lpf）;  //10hz *3.14 * 2*0.001


	if（ABS（ax）<4400 && ABS（ay）<4400 && ABS（az）<4400 ）
	{	
		//把加计的三维向量转成单位向量。
		ax = ax / norm_acc;//4096.0f;
		ay = ay / norm_acc;//4096.0f;
		az = az / norm_acc;//4096.0f; 
		
		if（ 3800 < norm_acc && norm_acc < 4400 ）
		{
			/* 叉乘得到误差 */
			ref.err_tmp.x = ay*reference_v.z - az*reference_v.y;
			ref.err_tmp.y = az*reference_