四旋翼无人机从0到1的实现（二十三）无人机工程中姿态解析

Author：家有仙妻谢掌柜
Date：2021/2/18

今年会更新一个系列，小四轴无人机从功能设计→思维导图→原理图设计→PCBLayout→焊接PCB→程序代码的编写→整机调试一系列，以此记录自己的成长历程！
这个小四轴无人机是大学时期学习制作的，加上现在工作学习对嵌入式的理解更加深入，因此想要重新梳理一下小四轴，之后在此基础上实现大四轴的飞控设计，这些都将在工作之余完成！

//小四轴无人机设计，姿态解算
 
#include "atititude_calculation.h"
/*******************************************************************************
 * fuction	invSqrt
 * brief	快速求解平方根的倒数
 * param	求解的数
 * return	求解结果
 *******************************************************************************/ 
static float invSqrt(float x)
{
    
    
	float halfx = 0.5f*x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df-(i>>1);
	y = *(float*)&i;
	y = y*(1.5f-(halfx*y*y));
	return y;
}
/*******************************************************************************
 * fuction	UpdateQ_GetEurAngle
 * brief	更新四元数，获取欧拉角
 * param	陀螺仪值 加速度计值 更新周期
 * return	无
 *******************************************************************************/ 
void UpdateQ_GetEurAngle(float_XYZ *gryo,float_XYZ *accel,float dt)
{
    
    
	float ax,ay,az;
	float gx,gy,gz;
	float recipNorm;
	float halfvx,halfvy,halfvz;
	float halfex,halfey,halfez;

	/* 获取加速度值 */
	ax = accel->X *100;                           //需要米每二次方秒  厘米每二次方秒  定高就很准
	ay = accel->Y *100;
	az = accel->Z *100;
	
	/* 获取陀螺仪值 */
	gx = gryo->X ;
	gy = gryo->Y ;
	gz = gryo->Z ;
	
	/* 将陀螺仪的值转化为弧度 */
	//1角速度 = 0.01745f
	gx *=DEG_TO_RAD;
	gy *=DEG_TO_RAD;
	gz *=DEG_TO_RAD;
	
	/* 将加速度的值归一化 */
	
	recipNorm =  invSqrt(ax*ax+ay*ay+az*az);
	ax *= recipNorm;
	ay *= recipNorm;
	az *= recipNorm;
	
	/* 提取四元数中的重力分量， */	                                       
	halfvx = q1*q3 - q0*q2;
	halfvy = q2*q3 + q0*q1;
	halfvz = 0.5f - q1*q1 - q2*q2;
	/* 求加速度的误差量 */    
	halfex = (ay*halfvz-az*halfvy);
	halfey = (az*halfvx-ax*halfvz);
	halfez = (ax*halfvy-ay*halfvx);
	
	/* 误差量输入PID控制器  得到一个修正的角速度 */
	gx += halfex*IMU_kp;
	gy += halfey*IMU_kp;
	gz += halfez*IMU_kp;
	
 /* 用修正的角速度去更新四元数 */
	q0 += 0.5f*dt*(-gx*q1-q2*gy-q3*gz);
	q1 += 0.5f*dt*(gx*q0+q2*gz-q3*gy);
	q2 += 0.5f*dt*(gx*q3+q0*gy-q1*gz);
	q3 += 0.5f*dt*(gy*q1-q2*gx+q0*gz);

 /* 四元数经过计算之后可能失去了规范性，因此需要在进行一次归一化 */
  	recipNorm = invSqrt(q0*q0+q1*q1+q2*q2+q3*q3);
  	q0 *= recipNorm;
	q1 *= recipNorm;
	q2 *= recipNorm;
	q3 *= recipNorm;
 /* 得到欧拉角 */
	Eur.pitch = asinf(2.0f*(q0*q2-q1*q3))*RAD_TO_DEG;
	Eur.roll = atan2f(2.0f*(q0*q1+q2*q3),q0*q0-q1*q1-q2*q2+q3*q3)*RAD_TO_DEG;
}

#ifndef _ATITITUDE_CALULATION_H__  
#define _ATITITUDE_CALULATION_H__

#include "board_define.h"
#include "var_global.h"
#define DEG_TO_RAD 0.01745f
#define RAD_TO_DEG 57.29578f
void UpdateQ_GetEurAngle(float_XYZ *gryo,float_XYZ *accel,float dt);
	
#endif