四旋翼姿态解算

代码思路如下：

/* 姿态解算 */
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)
{	
		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];
		
		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;
		
		
		if(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] *att_matrix[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方向向量，单位化。 */
    att_matrix[0][0] = imu->x_vec[X] = 1 - (2*q2q2 + 2*q3q3);
    att_matrix[0][1] = imu->x_vec[Y] = 2*q1q2 - 2*q0q3;
    att_matrix[0][2] = imu->x_vec[Z] = 2*q1q3 + 2*q0q2;
		
		/* 载体坐标下的y方向向量，单位化。 */
    att_matrix[1][0] = imu->y_vec[X] = 2*q1q2 + 2*q0q3;
    att_matrix[1][1] = imu->y_vec[Y] = 1 - (2*q1q1 + 2*q3q3);
    att_matrix[1][2] = imu->y_vec[Z] = 2*q2q3 - 2*q0q1;
		
    /* 载体坐标下的z方向向量（等效重力向量、重力加速度向量），单位化。 */
    att_matrix[2][0] = imu->z_vec[X] = 2*q1q3 - 2*q0q2;
    att_matrix[2][1] = imu->z_vec[Y] = 2*q2q3 + 2*q0q1;
    att_matrix[2][2] = imu->z_vec[Z] = 1 - (2*q1q1 + 2*q2q2);
		
		/* 水平面方向向量 */
		float hx_vec_reci = my_sqrt_reciprocal(my_pow(att_matrix[0][0]) + my_pow(att_matrix[1][0]));
		imu->hx_vec[X] = att_matrix[0][0] *hx_vec_reci;
		imu->hx_vec[Y] = att_matrix[1][0] *hx_vec_reci;
	
	
		/* 计算载体坐标下的运动加速度。(与姿态解算无关) */
		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] *att_matrix[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]);

		/* 误差积分 */
		vec_err_i[i] +=  LIMIT(vec_err[i],-0.1f,0.1f) *dT *ki_use;
			
#ifdef USE_MAG
			d_angle[i] = (gyr[i] + (vec_err[i]  + vec_err_i[i]) * kp_use + mag_yaw_err *imu->z_vec[i] *mkp_use) * dT / 2 ;
#else
			d_angle[i] = (gyr[i] + (vec_err[i]  + vec_err_i[i]) * kp_use ) * dT / 2 ;
#endif
		}
		
    /* 计算姿态。 */
    imu->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;
		
		
		if(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 = 10.0f;
				ki_use = 0.0f;
				
				/* 计算静态误差是否缩小 */
				imu_reset_val = (ABS(vec_err[X]) + ABS(vec_err[Y]));
				
				imu_reset_val = LIMIT(imu_reset_val,0,1.0f);
				
				if((imu_reset_val < 0.05f) && (state->M_reset == 0))
				{
					/* 计时 */
					reset_cnt += 2;
					if(reset_cnt>400)
					{
						reset_cnt = 0;
						state->G_reset = 0;		/* 已经对准，清除复位标记 */
					}
				}
				else
				{
					reset_cnt = 0;
				}
			}
		}
}