无名飞控姿态解算和控制（一）

无名飞控的姿态解算和控制

从imu和磁力计，气压计拿到数据后，进入AHRSUpdate_GraDes_Delayback函数，其中X_w_av,Y_w_av,Z_w_av来自陀螺仪，X_g_av,Y_g_av,Z_g_av来自加速度计，

输出为空

void AHRSUpdate_GraDes_Delayback(X_w_av,Y_w_av,Z_w_av,
X_g_av,Y_g_av,Z_g_av);

定义：看看代码有一个大致的了解

void AHRSUpdate_GraDes_Delayback(float gx, float gy, float gz, float ax, float ay, float az)
{
float recipNorm;							// 平方根
float s0, s1, s2, s3;						// 梯度下降算子求出来的姿态
float qDot1, qDot2, qDot3, qDot4;			// 四元数微分方程求得的姿态
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
float delta;
uint16 i=0;
Insert_Yaw();
for(i=Quad_Num-1;i>0;i--)
{
Quad_Buf[i][0]=Quad_Buf[i-1][0];
Quad_Buf[i][1]=Quad_Buf[i-1][1];
Quad_Buf[i][2]=Quad_Buf[i-1][2];
Quad_Buf[i][3]=Quad_Buf[i-1][3];
}
Quad_Buf[0][0]=att.q[0];
Quad_Buf[0][1]=att.q[1];
Quad_Buf[0][2]=att.q[2];
Quad_Buf[0][3]=att.q[3];
// 二阶毕卡法求解微分方程

gx*=GYRO_CALIBRATION_COFF;
gy*=GYRO_CALIBRATION_COFF;
gz*=GYRO_CALIBRATION_COFF;

Yaw_Gyro=gz;
Pitch_Gyro=gx;
Roll_Gyro=gy;

Gyro_Length=sqrt(Yaw_Gyro*Yaw_Gyro
+Pitch_Gyro*Pitch_Gyro
+Roll_Gyro*Roll_Gyro);

/* 转换为弧度制 */
gx = gx * PI / 180;
gy = gy * PI / 180;
gz = gz * PI / 180;

/* 四元数微分方程计算本次待矫正四元数 */
qDot1 = 0.5f * (-Quad_Buf[Quad_Delay][1] * gx - Quad_Buf[Quad_Delay][2] * gy - Quad_Buf[Quad_Delay][3] * gz);
qDot2 = 0.5f * (Quad_Buf[Quad_Delay][0] * gx + Quad_Buf[Quad_Delay][2] * gz - Quad_Buf[Quad_Delay][3] * gy);
qDot3 = 0.5f * (Quad_Buf[Quad_Delay][0] * gy - Quad_Buf[Quad_Delay][1] * gz + Quad_Buf[Quad_Delay][3] * gx);
qDot4 = 0.5f * (Quad_Buf[Quad_Delay][0] * gz + Quad_Buf[Quad_Delay][1] * gy - Quad_Buf[Quad_Delay][2] * gx);

/* 加速度计输出有效时,利用加速度计补偿陀螺仪 */
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{
recipNorm=invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
/* 避免重复运算 */
_2q0 = 2.0f * att.q[0];
_2q1 = 2.0f * att.q[1];
_2q2 = 2.0f * att.q[2];
_2q3 = 2.0f * att.q[3];
_4q0 = 4.0f * att.q[0];
_4q1 = 4.0f * att.q[1];
_4q2 = 4.0f * att.q[2];
_8q1 = 8.0f * att.q[1];
_8q2 = 8.0f * att.q[2];
q0q0 = att.q[0] * att.q[0];
q1q1 = att.q[1] * att.q[1];
q2q2 = att.q[2] * att.q[2];
q3q3 = att.q[3] * att.q[3];

/* 梯度下降算法,计算误差函数的梯度 */
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * att.q[1] - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * att.q[2] + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * att.q[3] - _2q1 * ax + 4.0f * q2q2 * att.q[3] - _2q2 * ay;

/* 梯度归一化 */
recipNorm=invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;

/* 补偿由四元数微分方程引入的姿态误差 */
qDot1 -= BETADEF * s0;
qDot2 -= BETADEF * s1;
qDot3 -= BETADEF * s2;
qDot4 -= BETADEF * s3;
}

/* 将四元数姿态导数积分,得到当前四元数姿态 */
/* 二阶毕卡求解微分方程 */
delta = (CNTLCYCLE * gx) * (CNTLCYCLE * gx) + (CNTLCYCLE * gy) * (CNTLCYCLE * gy) + (CNTLCYCLE * gz) * (CNTLCYCLE * gz);
att.q[0] = (1.0f - delta / 8.0f) * att.q[0] + qDot1 * CNTLCYCLE;
att.q[1] = (1.0f - delta / 8.0f) * att.q[1] + qDot2 * CNTLCYCLE;
att.q[2] = (1.0f - delta / 8.0f) * att.q[2] + qDot3 * CNTLCYCLE;
att.q[3] = (1.0f - delta / 8.0f) * att.q[3] + qDot4 * CNTLCYCLE;

/* 单位化四元数 */
recipNorm=invSqrt(att.q[0] * att.q[0] + att.q[1] * att.q[1] + att.q[2] * att.q[2] + att.q[3] * att.q[3]);
att.q[0] *= recipNorm;
att.q[1] *= recipNorm;
att.q[2] *= recipNorm;
att.q[3] *= recipNorm;

/* 四元数到欧拉角转换,转换顺序为Z-Y-X,参见<Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors>.pdf一文,P24 */
Pitch= atan2(2.0f * att.q[2] * att.q[3] + 2.0f * att.q[0] * att.q[1], -2.0f * att.q[1] * att.q[1] - 2.0f * att.q[2]* att.q[2] + 1.0f) * RAD2DEG;	// Pitch
Roll= asin(2.0f * att.q[0]* att.q[2]-2.0f * att.q[1] * att.q[3]) * RAD2DEG;																		// Roll
//att.angle[YAW] = atan2(2.0f * att.q[1] * att.q[2] + 2.0f * att.q[0] * att.q[3], -2.0f * att.q[3] * att.q[3] - 2.0f * att.q[2] * att.q[2] + 1.0f) * RAD2DEG;		// Yaw

/*偏航角一阶互补
att.angle[_YAW]+=Yaw_Gyro*dt;

if((Mag_Yaw>90 && att.angle[_YAW]<-90)
|| (Mag_Yaw<-90 && att.angle[_YAW]>90))
att.angle[_YAW] = -att.angle[_YAW] * 0.988f + Mag_Yaw * 0.012f;
else att.angle[_YAW] = att.angle[_YAW] * 0.988f + Mag_Yaw * 0.012f;

if(att.angle[_YAW]<0)   Yaw=att.angle[_YAW]+360;
else Yaw=att.angle[_YAW];*/

/*偏航角一阶互补*/
att.angle[_YAW]+=Yaw_Gyro*dt;

if((Mag_Yaw>90 && att.angle[_YAW]<-90)
|| (Mag_Yaw<-90 && att.angle[_YAW]>90))
att.angle[_YAW] = -att.angle[_YAW] * 0.988f + Mag_Yaw * 0.012f;
else att.angle[_YAW] = att.angle[_YAW] * 0.988f + Mag_Yaw * 0.012f;

if(att.angle[_YAW]<0)   Yaw=att.angle[_YAW]+360;
else Yaw=att.angle[_YAW];
Yaw_Correct=Yaw;//0~360
//Yaw-=180.0;//+-180

imuComputeRotationMatrix();
}

算法可参照博客点击打开链接，这篇博客详细推导了基于梯度下降法和一阶毕卡法求解微分方程的姿态结算算法，最后给出了Madgwick的代码，配合着看效果更好

梯度下降法中，通过加速度计的数据求出一组四元数

 ，然后通过四元数的微分方程，并使用陀螺仪得到的数据求解出另外一组四元数

 。因为在高速运动状态下，陀螺仪数据更可靠；而在低速运动状态下，加速度计数据更可靠。所以两组四元数分别乘以权重再相加，就得到了期望的输出结果

下面具体分析代码

下面看代码，首先赋值变量不多说，以备后面使用

float recipNorm;							// 平方根
float s0, s1, s2, s3;						// 梯度下降算子求出来的姿态
float qDot1, qDot2, qDot3, qDot4;			// 四元数微分方程求得的姿态
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
float delta;
uint16 i=0;

接着是调用函数Insert_Yaw()，得到磁力计测出来的偏航角

要注意的是初值float Mag_Yaw=0;

void Insert_Yaw(void)
{
Mag_Yaw=Mag_IST8310.Angle_Mag;
}

在取传感器数据的时候，定义了一个结构体，

typedef struct {
uint8_t Buf[6];
int16_t Mag_Data[3];
float thx;
float thy;
int16_t x;
int16_t y;
int16_t z;
float Angle_Mag;
}IST8310;

其中Mag_IST8310.Angle_Mag的值这样得到，

Mag_IST8310.Angle_Mag=atan2(Mag_IST8310.thx,Mag_IST8310.thy)*57.296;

接着就是回到AHRSUpdate_GraDes_Delayback下面是一个循环，首先是由变量定义，其中 定义一个常数#define Quad_Num  20 

for(i=Quad_Num-1;i>0;i--)
{
Quad_Buf[i][0]=Quad_Buf[i-1][0];
Quad_Buf[i][1]=Quad_Buf[i-1][1];
Quad_Buf[i][2]=Quad_Buf[i-1][2];
Quad_Buf[i][3]=Quad_Buf[i-1][3];
}
Quad_Buf[0][0]=att.q[0];
Quad_Buf[0][1]=att.q[1];
Quad_Buf[0][2]=att.q[2];
Quad_Buf[0][3]=att.q[3];

数据移位，将Quad_Buf的前一位付给后一位，将上一次得到的数据赋给最前面一位。

再次赋值：

gx*=GYRO_CALIBRATION_COFF;
gy*=GYRO_CALIBRATION_COFF;
gz*=GYRO_CALIBRATION_COFF;

Yaw_Gyro=gz;
Pitch_Gyro=gx;
Roll_Gyro=gy;

常数定义 #define GYRO_CALIBRATION_COFF 0.060976f

接下来求陀螺仪的模：

Gyro_Length=sqrt(Yaw_Gyro*Yaw_Gyro
+Pitch_Gyro*Pitch_Gyro
+Roll_Gyro*Roll_Gyro);

再将陀螺仪数据转化为弧度：
 /* 转换为弧度制 */
gx = gx * PI / 180;
gy = gy * PI / 180;
gz = gz * PI / 180;
接下来是一个四
1221a
元数方程

/* 四元数微分方程计算本次待矫正四元数 */
qDot1 = 0.5f * (-Quad_Buf[Quad_Delay][1] * gx - Quad_Buf[Quad_Delay][2] * gy - Quad_Buf[Quad_Delay][3] * gz);
qDot2 = 0.5f * (Quad_Buf[Quad_Delay][0] * gx + Quad_Buf[Quad_Delay][2] * gz - Quad_Buf[Quad_Delay][3] * gy);
qDot3 = 0.5f * (Quad_Buf[Quad_Delay][0] * gy - Quad_Buf[Quad_Delay][1] * gz + Quad_Buf[Quad_Delay][3] * gx);
qDot4 = 0.5f * (Quad_Buf[Quad_Delay][0] * gz + Quad_Buf[Quad_Delay][1] * gy - Quad_Buf[Quad_Delay][2] * gx); 
下来可以看做求解四元素微分方程，但实际不是，因为这里少一个时间常数，比较一下求解四元素微分方程的一阶龙哥库塔法，就能看出不同，这里得到的是四元数的导数

下面判断

if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))

当加速度数据有效时
recipNorm=invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

其中invSqrt函数是求一个数的平方根倒数的快速算法
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;
}

接着变量定义，好理解
/* 避免重复运算 */
_2q0 = 2.0f * att.q[0];
_2q1 = 2.0f * att.q[1];
_2q2 = 2.0f * att.q[2];
_2q3 = 2.0f * att.q[3];
_4q0 = 4.0f * att.q[0];
_4q1 = 4.0f * att.q[1];
_4q2 = 4.0f * att.q[2];
_8q1 = 8.0f * att.q[1];
_8q2 = 8.0f * att.q[2];
q0q0 = att.q[0] * att.q[0];
q1q1 = att.q[1] * att.q[1];
q2q2 = att.q[2] * att.q[2];
q3q3 = att.q[3] * att.q[3];
下面计算梯度：

/* 梯度下降算法,计算误差函数的梯度 */
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * att.q[1] - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * att.q[2] + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * att.q[3] - _2q1 * ax + 4.0f * q2q2 * att.q[3] - _2q2 * ay;
推导过程见点击打开链接，

详细过程可以参考数学分析，矩阵论的教材

/* 梯度归一化 */
recipNorm=invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;

好理解

/* 补偿由四元数微分方程引入的姿态误差 */
qDot1 -= BETADEF * s0;
qDot2 -= BETADEF * s1;
qDot3 -= BETADEF * s2;
qDot4 -= BETADEF * s3;互补滤波

/* 将四元数姿态导数积分,得到当前四元数姿态 */
/* 二阶毕卡求解微分方程 */
delta = (CNTLCYCLE * gx) * (CNTLCYCLE * gx) + (CNTLCYCLE * gy) * (CNTLCYCLE * gy) + (CNTLCYCLE * gz) * (CNTLCYCLE * gz);
att.q[0] = (1.0f - delta / 8.0f) * att.q[0] + qDot1 * CNTLCYCLE;
att.q[1] = (1.0f - delta / 8.0f) * att.q[1] + qDot2 * CNTLCYCLE;
att.q[2] = (1.0f - delta / 8.0f) * att.q[2] + qDot3 * CNTLCYCLE;
att.q[3] = (1.0f - delta / 8.0f) * att.q[3] + qDot4 * CNTLCYCLE;

这里利用陀螺仪的四元数推导出对应的四元数导数，然后根据加速度计和陀螺仪的数据求出梯度，将梯度和导数作融合，求出纠正后的四元数导数，替换二阶毕卡算法的右边第二项，然后四元数归一化。
求解过程可以看看点击打开链接的附件有求解方法



/* 单位化四元数 */
recipNorm=invSqrt(att.q[0] * att.q[0] + att.q[1] * att.q[1] + att.q[2] * att.q[2] + att.q[3] * att.q[3]);
att.q[0] *= recipNorm;
att.q[1] *= recipNorm;
att.q[2] *= recipNorm;
att.q[3] *= recipNorm;
//

/* 四元数到欧拉角转换,转换顺序为Z-Y-X,参见<Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors>.pdf一文,P24 */
Pitch= atan2(2.0f * att.q[2] * att.q[3] + 2.0f * att.q[0] * att.q[1], -2.0f * att.q[1] * att.q[1] - 2.0f * att.q[2]* att.q[2] + 1.0f) * RAD2DEG; // Pitch
Roll= asin(2.0f * att.q[0]* att.q[2]-2.0f * att.q[1] * att.q[3]) * RAD2DEG; 四元数单位化，并转化成欧拉角，俯仰和横滚

/*偏航角一阶互补*/
att.angle[_YAW]+=Yaw_Gyro*dt;

if((Mag_Yaw>90 && att.angle[_YAW]<-90)
|| (Mag_Yaw<-90 && att.angle[_YAW]>90))
att.angle[_YAW] = -att.angle[_YAW] * 0.988f + Mag_Yaw * 0.012f;
else att.angle[_YAW] = att.angle[_YAW] * 0.988f + Mag_Yaw * 0.012f;

if(att.angle[_YAW]<0) Yaw=att.angle[_YAW]+360;
else Yaw=att.angle[_YAW];
Yaw_Correct=Yaw;//0~360
//Yaw-=180.0;//+-180

得偏航角
下面：

imuComputeRotationMatrix();

void imuComputeRotationMatrix(void)
{
float q1q1,q2q2,q3q3;
float q0q1,q0q2,q0q3,q1q2,q1q3,q2q3;

q0_DCM=att.q[0];
q1_DCM=att.q[1];
q2_DCM=att.q[2];
q3_DCM=att.q[3];

q1q1 = sq(q1_DCM );
q2q2 = sq(q2_DCM );
q3q3 = sq(q3_DCM );

q0q1 = q0_DCM  * q1_DCM ;
q0q2 = q0_DCM  * q2_DCM ;
q0q3 = q0_DCM  * q3_DCM ;
q1q2 = q1_DCM  * q2_DCM ;
q1q3 = q1_DCM  * q3_DCM ;
q2q3 = q2_DCM  * q3_DCM ;

rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
rMat[0][1] = 2.0f * (q1q2 -q0q3);
rMat[0][2] = 2.0f * (q1q3 +q0q2);

rMat[1][0] = 2.0f * (q1q2 +q0q3);
rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
rMat[1][2] = 2.0f * (q2q3 -q0q1);

rMat[2][0] = 2.0f * (q1q3 -q0q2);
rMat[2][1] = 2.0f * (q2q3 +q0q1);
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}

根据纠正后的四元数得到旋转矩阵，有机体系到惯性系
接着回到TIME.C

float Gyro_X,Gyro_Y,Gyro_Z;
float Angle_X,Angle_Y,Angle_Z;
float ACCE_X,ACCE_Y,ACCE_Z;
void Angle_Calculate(void)//角度计算
{
float ACCE_X_TEMP,ACCE_Y_TEMP,ACCE_Z_TEMP;
ACCE_X_TEMP=ACCE_X=X_g_av;
ACCE_Y_TEMP=ACCE_Y=Y_g_av;
ACCE_Z_TEMP=ACCE_Z=Z_g_av;
ACCE_X=-57.3*atan(ACCE_X_TEMP*invSqrt(ACCE_Y_TEMP*ACCE_Y_TEMP+ACCE_Z_TEMP*ACCE_Z_TEMP));
ACCE_Y=57.3*atan(ACCE_Y_TEMP*invSqrt(ACCE_X_TEMP*ACCE_X_TEMP+ACCE_Z_TEMP*ACCE_Z_TEMP));
ACCE_Z=57.3*atan(sqrt(ACCE_X_TEMP*ACCE_X_TEMP+ACCE_Y_TEMP*ACCE_Y_TEMP)/ACCE_Z_TEMP);
}有加速度计算欧拉角，角度
下面有

HC_SR04_Cnt++;
if(HC_SR04_Cnt>=40)//200ms
{
HC_SR04_Start();
HC_SR04_Cnt=0;
}HC_SR04_Cnt计数40次以后进入HC_SR04_Start

uint8 HC_SR04_StartFlag=0;
float HC_SR04_Distance=0;
void HC_SR04_Start(void)
{
if(HC_SR04_StartFlag==0)
{
HC_SR04_OUT_HIGH;
delay_us(20);
HC_SR04_OUT_LOW;
EXTI->IMR |=EXTI_Line1;
HC_SR04_StartFlag=1;
}

打开HC_SR04的外部中断
这里有必要分析一下HC_SR04的中断函数
u32 Test_Cnt1,Test_Cnt2=0,Test_Delta=0;
uint16 Exti_Cnt=0;
uint16 Sample_Cnt=0;
void EXTI1_IRQHandler(void)
{
if(EXTI_GetITStatus(EXTI_Line1) != RESET)判断中段发生
{
Sample_Cnt++;加一等于1
if(Sample_Cnt==1)
{
Exti_Cnt++;
Test_Cnt1=10000*TIME_ISR_CNT
+TIM2->CNT/2;到现在为止过了多少ms
HC_SR04_DN();中断初始化
}
else
{
Test_Cnt2=10000*TIME_ISR_CNT
+TIM2->CNT/2;
HC_SR04_StartFlag=0;
HC_SR04_UP();
EXTI->IMR &=~EXTI_Line1;//关闭外部中断
Sample_Cnt=0;
//HC_SR04_Distance=(Test_Cnt2-Test_Cnt1)*0.017/2;
//HC_SR04_Distance=(Test_Cnt2-Test_Cnt1)*(331.4+0.607*DHT11_Data[1])/40000;
//HC_SR04_Distance=(Test_Cnt2-Test_Cnt1)*(325*100/2)*0.5/1000000;
Test_Delta=(Test_Cnt2-Test_Cnt1);
HC_SR04_Distance=(Test_Cnt2-Test_Cnt1)*(325)/20000.0;
}
}
EXTI_ClearITPendingBit(EXTI_Line1);
}

SINS_Prepare();//得到载体相对导航系的三轴运动加速度

void  SINS_Prepare(void)
{
Vector2f SINS_Accel_Earth={0,0};
Sin_Pitch=sin(Pitch* DEG2RAD);
Cos_Pitch=cos(Pitch* DEG2RAD);
Sin_Roll=sin(Roll* DEG2RAD);
Cos_Roll=cos(Roll* DEG2RAD);
Sin_Yaw=sin(Yaw* DEG2RAD);
Cos_Yaw=cos(Yaw* DEG2RAD);
/*Z-Y-X欧拉角转方向余弦矩阵
//Pitch Roll  Yaw 分别对应Φ θ Ψ

X轴旋转矩阵
R（Φ）
{1      0        0    }
{0      cosΦ    sinΦ}
{0    -sinΦ    cosΦ }

Y轴旋转矩阵
R（θ）
{cosθ     0        -sinθ}
{0         1        0     }
{sinθ     0        cosθ}

Z轴旋转矩阵
R（θ）
{cosΨ      sinΨ       0}
{-sinΨ     cosΨ       0}
{0          0           1 }

由Z-Y-X顺规有:
载体坐标系到导航坐标系下旋转矩阵R(b2n)
R(b2n) =R(Ψ)^T*R(θ)^T*R(Φ)^T

R=
{cosΨ*cosθ     -cosΦ*sinΨ+sinΦ*sinθ*cosΨ        sinΨ*sinΦ+cosΦ*sinθ*cosΨ}
{cosθ*sinΨ     cosΦ*cosΨ +sinΦ*sinθ*sinΨ       -cosΨ*sinΦ+cosΦ*sinθ*sinΨ}
{-sinθ          cosθsin Φ                          cosθcosΦ                   }
*/

Origion_NamelessQuad.Acceleration[_YAW] =
-Sin_Roll* Acce_Control[0]
+ Sin_Pitch *Cos_Roll * Acce_Control[1]
+ Cos_Pitch * Cos_Roll *Acce_Control[2];

Origion_NamelessQuad.Acceleration[_PITCH]=
Cos_Yaw* Cos_Roll * Acce_Control[0]
+(Sin_Pitch*Sin_Roll*Cos_Yaw-Cos_Pitch * Sin_Yaw) * Acce_Control[1]
+(Sin_Pitch * Sin_Yaw+Cos_Pitch * Sin_Roll * Cos_Yaw)*Acce_Control[2];

Origion_NamelessQuad.Acceleration[_ROLL]=
Sin_Yaw* Cos_Roll * Acce_Control[0]
+(Sin_Pitch * Sin_Roll * Sin_Yaw +Cos_Pitch * Cos_Yaw) * Acce_Control[1]
+ (Cos_Pitch * Sin_Roll * Sin_Yaw - Sin_Pitch * Cos_Yaw)*Acce_Control[2];

Origion_NamelessQuad.Acceleration[_YAW]*=AcceGravity/AcceMax;
Origion_NamelessQuad.Acceleration[_YAW]-=AcceGravity;//减去重力加速度
Origion_NamelessQuad.Acceleration[_YAW]*=100;//加速度cm/s^2

Origion_NamelessQuad.Acceleration[_PITCH]*=AcceGravity/AcceMax;
Origion_NamelessQuad.Acceleration[_PITCH]*=100;//加速度cm/s^2

Origion_NamelessQuad.Acceleration[_ROLL]*=AcceGravity/AcceMax;
Origion_NamelessQuad.Acceleration[_ROLL]*=100;//加速度cm/s^2

Acceleration_Length=sqrt(Origion_NamelessQuad.Acceleration[_YAW]*Origion_NamelessQuad.Acceleration[_YAW]
+Origion_NamelessQuad.Acceleration[_PITCH]*Origion_NamelessQuad.Acceleration[_PITCH]
+Origion_NamelessQuad.Acceleration[_ROLL]*Origion_NamelessQuad.Acceleration[_ROLL]);

/******************************************************************************/
//将无人机在导航坐标系下的沿着正东、正北方向的运动加速度旋转到当前航向的运动加速度:机头(俯仰)+横滚

SINS_Accel_Earth.x=Origion_NamelessQuad.Acceleration[_PITCH];//沿地理坐标系，正东方向运动加速度,单位为CM
SINS_Accel_Earth.y=Origion_NamelessQuad.Acceleration[_ROLL];//沿地理坐标系，正北方向运动加速度,单位为CM

SINS_Accel_Body.x=SINS_Accel_Earth.x*Cos_Yaw+SINS_Accel_Earth.y*Sin_Yaw;  //横滚正向运动加速度  X轴正向
SINS_Accel_Body.y=-SINS_Accel_Earth.x*Sin_Yaw+SINS_Accel_Earth.y*Cos_Yaw; //机头正向运动加速度  Y轴正向

}

得到旋转矩阵后，将滤波后加速度值转化到惯性系，并得到此时机体各轴方向的加速度
接下来

if(High_Okay_flag==1)//高度惯导融合
{
Strapdown_INS_High();
//Strapdown_INS_High_Kalman();
}高度互补滤波
float pos_correction[3]={0,0,0};
float acc_correction[3]={0,0,0};
float vel_correction[3]={0,0,0};
/****气压计三阶互补滤波方案——参考开源飞控APM****/
//#define TIME_CONTANST_ZER 1.5f
float TIME_CONTANST_ZER=3.0f;
#define K_ACC_ZER (1.0f / (TIME_CONTANST_ZER * TIME_CONTANST_ZER * TIME_CONTANST_ZER))
#define K_VEL_ZER (3.0f / (TIME_CONTANST_ZER * TIME_CONTANST_ZER))//20 // XY????·′à??μêy,3.0
#define K_POS_ZER (3.0f / TIME_CONTANST_ZER)
//#define High_Delay_Cnt 5 //20ms
uint16 High_Delay_Cnt=1;
float Altitude_Dealt=0;
float Altitude_Estimate=0;
void Strapdown_INS_High()
{
uint16 Cnt=0;
static uint16_t Save_Cnt=0;
Save_Cnt++;//数据存储周期
#ifdef IMU_BOARD_GY86
Altitude_Estimate=AirPresure_Altitude;//高度观测量
#elsedef IMU_BOARD_NC686
Altitude_Estimate=SPL06_001_Filter_high;
#elsedef IMU_BOARD_NC683
Altitude_Estimate=FBM320_Filter_high;
#endif
//由观测量（气压计）得到状态误差
Altitude_Dealt=Altitude_Estimate-NamelessQuad.Pos_History[_YAW][High_Delay_Cnt];//气压计(超声波)与SINS估计量的差，单位cm
//三路积分反馈量修正惯导
acc_correction[_YAW] +=Altitude_Dealt* K_ACC_ZER*CNTLCYCLE ;//加速度矫正量
vel_correction[_YAW] +=Altitude_Dealt* K_VEL_ZER*CNTLCYCLE ;//速度矫正量
pos_correction[_YAW] +=Altitude_Dealt* K_POS_ZER*CNTLCYCLE ;//位置矫正量
//加速度计矫正后更新
NamelessQuad.Acceleration[_YAW]=Origion_NamelessQuad.Acceleration[_YAW]+acc_correction[_YAW];
//速度增量矫正后更新，用于更新位置
SpeedDealt[_YAW]=NamelessQuad.Acceleration[_YAW]*CNTLCYCLE;
//原始位置更新
Origion_NamelessQuad.Position[_YAW]+=(NamelessQuad.Speed[_YAW]+0.5*SpeedDealt[_YAW])*CNTLCYCLE;
//位置矫正后更新
NamelessQuad.Position[_YAW]=Origion_NamelessQuad.Position[_YAW]+pos_correction[_YAW];
//原始速度更新
Origion_NamelessQuad.Speed[_YAW]+=SpeedDealt[_YAW];
//速度矫正后更新
NamelessQuad.Speed[_YAW]=Origion_NamelessQuad.Speed[_YAW]+vel_correction[_YAW];

if(Save_Cnt>=5)//20ms
{
for(Cnt=Num-1;Cnt>0;Cnt--)//20ms滑动一次
{
NamelessQuad.Pos_History[_YAW][Cnt]=NamelessQuad.Pos_History[_YAW][Cnt-1];
}
NamelessQuad.Pos_History[_YAW][0]=NamelessQuad.Position[_YAW];
Save_Cnt=0;
}

}

气压计三阶互补融合
解说可以点击打开链接

接下来是对于GPS的使用

if(GPS_ISR_CNT>=300
&&GPS_Update_finished==1)//GPS_PVT语句更新完毕后，开始解析
{
GPS_PVT_Parse();//GPS串口数据帧解析
GPS_Update_finished=0;
Set_GPS_Home();//设置Home点
}
if(GPS_Sate_Num>=9 //定位星数
&&GPS_Quality<=3.0//定位信号质量,有效定位
&&GPS_Home_Set==1)//Home点已设置
{
//Strapdown_INS_Horizontal();
Filter_Horizontal();
Bling_Set(&Light_4,2000,1000,0.5,0,GPIOA,WORK_LED,1);
}在飞控的硬件见图里GPS接的USART3，每引发一次串口接受中断，GPS_ISR_CNT自加一次直到2000

u8 GPS_Buf[2][100]={0};
unsigned char GPS_GPGGA_Buf[6];
unsigned int GPS_Data_Cnt=0;
unsigned GPSx_Cnt=0,GPSx_Finish_Flag=1,GPSx_Data_Cnt=0;
u16 GPS_ISR_CNT=0;
uint8_t GPxCnt=0;

uint16 Ublox_Try_Cnt=0;
uint8 Ublox_Try_Buf[5]={0};
uint16 Ublox_Try_Makesure=0;
uint16 Ublox_Try_Start=0;

uint8 Ublox_Data[95]={0};
uint16 Ublox_Cnt=0;
uint16 Ublox_Receive=0;
uint16 GPS_Update_finished=0;

Testime GPS_Time_Delta;
void USART3_IRQHandler(void)
{
unsigned char ch;
if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
{

if(GPS_ISR_CNT<=2000)
{
GPS_ISR_CNT++;
}

ch=USART_ReceiveData(USART3);

if(Ublox_Try_Makesure==1)
{
Ublox_Data[Ublox_Cnt++]=ch;
if(Ublox_Cnt==94)
{
Ublox_Cnt=0;
Ublox_Try_Makesure=0;
GPS_Update_finished=1;
Test_Period(&GPS_Time_Delta);//GPS���ݸ��¼�����
}
}

if(Ublox_Try_Makesure==0
&&ch==0xB5)
{
Ublox_Try_Start=1;
Ublox_Try_Cnt=0;
}

if(Ublox_Try_Start==1)
{
Ublox_Try_Cnt++;
if(Ublox_Try_Cnt>=5)
{
Ublox_Try_Start=0;
Ublox_Try_Cnt=0;

if(ch==0x5C) Ublox_Try_Makesure=1;
else Ublox_Try_Makesure=0;
}
}

}
USART_ClearITPendingBit(USART3, USART_IT_RXNE);

}

 另外Set_GPS_Home设定当前的经纬度为家的经纬度，并GPS_Home_Set=1

uint8 GPS_Home_Set=0; void Set_GPS_Home(void) { if(GPS_Home_Set==0&&GPS_Sate_Num>=5) { GPSData_To_XY_Home[_PITCH]=Longitude; GPSData_To_XY_Home[_ROLL]=Latitude; GPS_Home_Set=1;//设定返航点 } }下面的是以前写的，删了吧可惜，就留下吧

继续判断：  if(GPS_ISR_CNT>=300
     if(GPS_ISR_CNT>=300 &&GPS_Update_finished==1)//GPS_PVT语句更新完毕后，开始解析
{
GPS_PVT_Parse();//GPS串口数据帧解析
GPS_Update_finished=0;
Set_GPS_Home();//设置Home点
}跳到USAET.c

u8 GPS_Buf[2][100]={0};
unsigned char GPS_GPGGA_Buf[6];
unsigned int GPS_Data_Cnt=0;
unsigned GPSx_Cnt=0,GPSx_Finish_Flag=1,GPSx_Data_Cnt=0;
u16 GPS_ISR_CNT=0;
uint8_t GPxCnt=0;

uint16 Ublox_Try_Cnt=0;
uint8 Ublox_Try_Buf[5]={0};
uint16 Ublox_Try_Makesure=0;
uint16 Ublox_Try_Start=0;

uint8 Ublox_Data[95]={0};
uint16 Ublox_Cnt=0;
uint16 Ublox_Receive=0;
uint16 GPS_Update_finished=0;

Testime GPS_Time_Delta;
void USART3_IRQHandler(void)
{
unsigned char ch;
if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)//检查指定的USART中断发生与否，USARTx：x可以是1，2或者3，
{ //USART_IT_RXNE接收中断

if(GPS_ISR_CNT<=2000)
{
GPS_ISR_CNT++;
}

ch=USART_ReceiveData(USART3);//返回USART3最近接收到的数据

if(Ublox_Try_Makesure==1)//uint16 Ublox_Try_Makesure=0;
{
Ublox_Data[Ublox_Cnt++]=ch;
if(Ublox_Cnt==94)
{
Ublox_Cnt=0;
Ublox_Try_Makesure=0;
GPS_Update_finished=1;
Test_Period(&GPS_Time_Delta);//GPS数据更新间隔测试
}
}

if(Ublox_Try_Makesure==0
&&ch==0xB5)
{
Ublox_Try_Start=1;
Ublox_Try_Cnt=0;
}

if(Ublox_Try_Start==1)
{
Ublox_Try_Cnt++;
if(Ublox_Try_Cnt>=5)
{
Ublox_Try_Start=0;
Ublox_Try_Cnt=0;

if(ch==0x5C) Ublox_Try_Makesure=1;
else Ublox_Try_Makesure=0;
}
}

}
USART_ClearITPendingBit(USART3, USART_IT_RXNE);//清除USARTx的中断待处理位

}

其中函数Test_Period

void Test_Period(Testime *Time_Lab)
{
Time_Lab->Last_Time=Time_Lab->Now_Time;
Time_Lab->Now_Time=(10000*TIME_ISR_CNT
+TIM2->CNT/2)/1000.0;//单位ms
Time_Lab->Time_Delta=Time_Lab->Now_Time-Time_Lab->Last_Time;
Time_Lab->Time_Delta_INT=(uint16)(Time_Lab->Time_Delta);
}

GPS_PVT语句更新完毕后，开始解析

然后： GPS_PVT_Parse();//GPS串口数据帧解析

u8 GpsFlag;
double Last_Longitude=0,Last_Latitude=0;
double Longitude,Latitude;
double Longitude_Deg,Latitude_Deg;
double Longitude_Min,Latitude_Min;
float GPS_Speed=0;
float GPS_Yaw=0;
float GPS_Quality=0;
uint16 GPS_Sate_Num=0;
float GPS_Speed_Resolve[2]={0,0};
u16 TimeBeijing[3];
char TimeStr[8];
float GPS_Vel[3]={0};
float GPS_Pos_DOP=0;
uint8 GPS_FixType=0;
uint8 GPS_Fix_Flag[4]={0};
int16 Horizontal_Acc_Est=0;//水平位置精度
int16 Vertical_Acc_Est=0;//竖直位置精度
int16 Speed_Acc_Est=0;//竖直位置精度
float High_GPS=0;//海拔高度
void GPS_PVT_Parse(void)
{

Last_Longitude=Longitude;
Last_Latitude=Latitude;

GPS_FixType=Ublox_Data[21];//定位类型
GPS_Fix_Flag[0]=Ublox_Data[23]&0x01;//有效定位
GPS_Fix_Flag[1]=(Ublox_Data[23]&0x02)>>1;
GPS_Fix_Flag[2]=(Ublox_Data[23]&0x3A)>>2;
GPS_Fix_Flag[3]=Ublox_Data[23]&0x20;
GPS_Sate_Num=Ublox_Data[24];

Longitude=Ublox_Data[25]
+(Ublox_Data[26]<<8)
+(Ublox_Data[27]<<16)
+(Ublox_Data[28]<<24);
Longitude*=0.0000001f;//deg

Latitude=Ublox_Data[29]
+(Ublox_Data[30]<<8)
+(Ublox_Data[31]<<16)
+(Ublox_Data[32]<<24);
Latitude*=0.0000001f;//deg

Longitude_Deg=(int)(Longitude);
Longitude_Min=((int)((Longitude-Longitude_Deg)*10000000));

Latitude_Deg=(int)(Latitude);
Latitude_Min=((int)((Latitude-Latitude_Deg)*10000000));

High_GPS=Ublox_Data[37]
+(Ublox_Data[38]<<8)
+(Ublox_Data[39]<<16)
+(Ublox_Data[40]<<24);
High_GPS/=1000;//m

Horizontal_Acc_Est=Ublox_Data[41]
+(Ublox_Data[42]<<8)
+(Ublox_Data[43]<<16)
+(Ublox_Data[44]<<24);;
Horizontal_Acc_Est*=0.01;//m

Vertical_Acc_Est=Ublox_Data[45]
+(Ublox_Data[46]<<8)
+(Ublox_Data[47]<<16)
+(Ublox_Data[48]<<24);;
Vertical_Acc_Est*=0.01;//m

GPS_Vel[_PITCH]=Ublox_Data[49]
+(Ublox_Data[50]<<8)
+(Ublox_Data[51]<<16)
+(Ublox_Data[52]<<24);
GPS_Vel[_PITCH]/=10;//cm/s  N
GPS_Vel[_PITCH]*=-1.0;

GPS_Vel[_ROLL]=Ublox_Data[53]
+(Ublox_Data[54]<<8)
+(Ublox_Data[55]<<16)
+(Ublox_Data[56]<<24);
GPS_Vel[_ROLL]/=10;//cm/s  E
GPS_Vel[_ROLL]*=-1.0;
GPS_Vel[_YAW]=Ublox_Data[57]
+(Ublox_Data[58]<<8)
+(Ublox_Data[59]<<16)
+(Ublox_Data[60]<<24);
GPS_Vel[_YAW]/=10;//cm/s  D

GPS_Speed_Resolve[0]=GPS_Vel[_PITCH];//Y  Axis
GPS_Speed_Resolve[1]=GPS_Vel[_ROLL];//X  Axis

Speed_Acc_Est=Ublox_Data[69]
+(Ublox_Data[70]<<8)
+(Ublox_Data[71]<<16)
+(Ublox_Data[72]<<24);;
Speed_Acc_Est*=0.1;//cm/s
GPS_Pos_DOP=Ublox_Data[77]
+(Ublox_Data[78]<<8);
GPS_Pos_DOP*=0.01;
GPS_Quality=GPS_Pos_DOP;

TimeBeijing[0]=Ublox_Data[9]+8;//时
TimeBeijing[1]=Ublox_Data[10];//分
TimeBeijing[2]=Ublox_Data[11];//秒
}

然后赋值：

 GPS_Update_finished=0;

 Set_GPS_Home();//设置Home点

uint8 GPS_Home_Set=0;
void Set_GPS_Home(void)
{
if(GPS_Home_Set==0&&GPS_Sate_Num>=5)
{
GPSData_To_XY_Home[_PITCH]=Longitude;
GPSData_To_XY_Home[_ROLL]=Latitude;
GPS_Home_Set=1;//设定返航点
}
}

完成后在进入判断：

if(GPS_Sate_Num>=5
&&GPS_Home_Set==1)//有效定位
{
Strapdown_INS_Horizontal();
Bling_Set(&Light_4,2000,1000,0.5,0,GPIOA,WORK_LED,1);//#define WORK_LED GPIO_Pin_15
}

若判断成立；进入函数

void Strapdown_INS_Horizontal()
{
GPSData_Sort();

//GPS导航坐标系相对位置与SINS估计量的差，单位cm
X_Dealt=GPSData_To_XY[_PITCH]-NamelessQuad.Position[_PITCH];
Y_Dealt=GPSData_To_XY[_ROLL]-NamelessQuad.Position[_ROLL];

acc_correction[_PITCH] += X_Dealt* K_ACC_XY*CNTLCYCLE;//加速度矫正量
vel_correction[_PITCH] += X_Dealt* K_VEL_XY*CNTLCYCLE;//速度矫正量
pos_correction[_PITCH] += X_Dealt* K_POS_XY*CNTLCYCLE;//位置矫正量

acc_correction[_ROLL] += Y_Dealt* K_ACC_XY*CNTLCYCLE;//加速度矫正量
vel_correction[_ROLL] += Y_Dealt* K_VEL_XY*CNTLCYCLE;//速度矫正量
pos_correction[_ROLL] += Y_Dealt* K_POS_XY*CNTLCYCLE;//位置矫正量

/*************************************************************/
//水平运动加速度计校正
NamelessQuad.Acceleration[_PITCH]=Origion_NamelessQuad.Acceleration[_PITCH]+acc_correction[_PITCH];
//速度增量矫正后更新，用于更新位置
SpeedDealt[_PITCH]=NamelessQuad.Acceleration[_PITCH]*CNTLCYCLE;
//原始位置更新
Origion_NamelessQuad.Position[_PITCH]+=(NamelessQuad.Speed[_PITCH]+0.5*SpeedDealt[_PITCH])*CNTLCYCLE;
//位置矫正后更新
NamelessQuad.Position[_PITCH]=Origion_NamelessQuad.Position[_PITCH]+pos_correction[_PITCH];
//原始速度更新
Origion_NamelessQuad.Speed[_PITCH]+=SpeedDealt[_PITCH];
//速度矫正后更新
NamelessQuad.Speed[_PITCH]=Origion_NamelessQuad.Speed[_YAW]+vel_correction[_PITCH];

/*************************************************************/
//水平运动加速度计校正
NamelessQuad.Acceleration[_ROLL]=Origion_NamelessQuad.Acceleration[_ROLL]+acc_correction[_ROLL];
//速度增量矫正后更新，用于更新位置
SpeedDealt[_ROLL]=NamelessQuad.Acceleration[_ROLL]*CNTLCYCLE;
//原始位置更新
Origion_NamelessQuad.Position[_ROLL]+=(NamelessQuad.Speed[_ROLL]+0.5*SpeedDealt[_ROLL])*CNTLCYCLE;
//位置矫正后更新
NamelessQuad.Position[_ROLL]=Origion_NamelessQuad.Position[_ROLL]+pos_correction[_ROLL];
//原始速度更新
Origion_NamelessQuad.Speed[_ROLL]+=SpeedDealt[_ROLL];
//速度矫正后更新
NamelessQuad.Speed[_ROLL]=Origion_NamelessQuad.Speed[_YAW]+vel_correction[_ROLL];
}

这个函数之前定义了一些变量：

#define TIME_CONTANST_XY      2.5f
#define K_ACC_XY	     (1.0f / (TIME_CONTANST_XY * TIME_CONTANST_XY * TIME_CONTANST_XY))
#define K_VEL_XY             (3.0f / (TIME_CONTANST_XY * TIME_CONTANST_XY))															// XY????·′à??μêy,3.0
#define K_POS_XY             (3.0f / TIME_CONTANST_XY)
float X_Dealt=0,Y_Dealt=0;

首先进入函数：GPSData_Sort

float GPSData_To_XY[3];
float GPSData_To_XY_Home[3]={0,114.363746,30.627600};//经、纬度
float GPSDataFilter[2][10]={0};
void GPSData_Sort()
{
GPSData_To_XY[_PITCH]=(float)(Longitude-GPSData_To_XY_Home[_PITCH])*LON_TO_CM;//经度
GPSData_To_XY[_ROLL]=(float)(Latitude-GPSData_To_XY_Home[_ROLL])*LAT_TO_CM;//纬度
}

其中：#define LON_TO_CM  (2.0f * WGS84_RADIUS_EQUATOR * PI*LON_COSINE_LOCAL/360.0f)*100.0f

算出GPSData_To_XY[i],i=1,2

其中：NamelessQuad 是一个SINS的结构体

typedef struct
{
float Position[Axis_Num];//位置估计量
float Speed[Axis_Num];//速度估计量
float Acceleration[Axis_Num];//加速度估计量
float Pos_History[Axis_Num][Num];//历史惯导位置
float Vel_History[Axis_Num][Num];//历史惯导速度
float Acce_Bias[Axis_Num];//惯导加速度漂移量估计量
}SINS;

这一部分详见博客从APM源码分析GPS、气压计惯导融

然后回到

Bling_Set(&Light_4,2000,1000,0.5,0,GPIOA,WORK_LED,1);//#define WORK_LED GPIO_Pin_15

详细定义

Bling_Light Light_1,Light_2,Light_3,Light_4;
uint16_t Bling_Mode=0;
void Bling_Set(Bling_Light *Light,//&Light_4
uint32_t Continue_time,//持续时间/
uint16_t Period,//周期100ms~1000ms
float Percent,//0~100%
uint16_t  Cnt,
GPIO_TypeDef* Port,
uint16_t Pin
,uint8_t Flag)
{
Light->Bling_Contiune_Time=Continue_time/4;//持续时间
Light->Bling_Period=Period;//周期
Light->Bling_Percent=Percent;//占空比
Light->Bling_Cnt=Cnt;
Light->Port=Port;//端口
Light->Pin=Pin;//引脚
Light->Endless_Flag=Flag;//无尽模式
}

Light_4是一个结构体

typedef struct
{
uint16_t Bling_Contiune_Time;//闪烁持续时间
uint16_t Bling_Period;//闪烁周期
float  Bling_Percent;//闪烁占空比
uint16_t  Bling_Cnt;//闪烁计数器
GPIO_TypeDef* Port; //端口
uint16_t Pin;//引脚
uint8_t Endless_Flag;//无尽模式
}Bling_Light;

然后回到TIME.c

Total_Control();//总控制器

打开定义

void Total_Control(void)
{
static uint16_t Mode_Check_Cnt=0;
/*************控制器模式选着******************/
Mode_Check_Cnt++;
if(Mode_Check_Cnt>=5)//每20ms检测一次，PPM信号刷新周期为20ms
{
Controler_Mode_Select();
Mode_Check_Cnt=0;
}
/*************主导控制器******************/
Main_Leading_Control();
/*************姿态环控制器*****************/
Altitude_Control();
}

先进入Controler_Mode_Select

void Controler_Mode_Select()
{
Last_Controler_Mode=Controler_Mode;//上次控制器模式
if(PPM_Databuf[6]>=1900)      Controler_Mode=3;//GPS定点
else if(PPM_Databuf[6]>=1400) Controler_Mode=2;//气压计定高
else if(PPM_Databuf[6]>=900)  Controler_Mode=1;//纯姿态自稳

if(Controler_Mode!=Last_Controler_Mode)
{
if(Controler_Mode==3)  {Control_Mode_Change=2;Pos_Hold_SetFlag=0;}//定高切定点
if(Controler_Mode==2)  {Control_Mode_Change=1;High_Hold_SetFlag=0;}//自稳切定高
}
else Control_Mode_Change=0;//无模式切换

if(Controler_Mode==High_Hold_Mode//本次为定高模式
&&Last_Controler_Mode==Self_Balance_Mode//上次为自稳模式
&&High_Hold_SetFlag==0)//高度只设置一次
{
High_Hold_Throttle=1360;//保存当前油门值，只存一次
/*******************将当前惯导竖直位置估计作为目标高度***************************/
Total_Controler.High_Position_Control.Expect
=NamelessQuad.Position[_YAW];//将开关拨动瞬间的惯导高度设置为期望高度
High_Hold_SetFlag=1;
}

if(Controler_Mode==Pos_Hold_Mode//本次为定点模式
&&Last_Controler_Mode==High_Hold_Mode//上次为定高模式
&&Pos_Hold_SetFlag==0
&&GPS_Sate_Num>=6//定位卫星超过6个
&&GPS_Quality<=5)//水平精度因子大于6不可用,水平位置期望只设置一次
{
/*******************将当前惯导水平位置估计作为目标悬停点************************/
Total_Controler.Latitude_Position_Control.Expect
=NamelessQuad.Position[_ROLL];
Total_Controler.Longitude_Position_Control.Expect
=NamelessQuad.Position[_PITCH];
Pos_Hold_SetFlag=1;
}
/******当前档位为定点模式，但显示悬停点未设置，说明之前未满足设置定点条件有三种情况********
1、初始通过开关切定点模式时，GPS状态未满足悬停条件；
2、初始通过开关切定点模式时，GPS状态未满足悬停条件，之后持续检测仍然未满足GPS定点条件；
3、之前GPS状态满足悬停条件，但由于GPS信号质量变差，自动切换至不满足GPS定点条件；
*******重新判断当下是否满足定点条件，如满足条件更新悬停点，允许进入定点模式******/
if(Controler_Mode==Pos_Hold_Mode
&&Pos_Hold_SetFlag==0)
{
if(GPS_Sate_Num>=6//定位卫星超过6个
&&GPS_Quality<=5)//水平精度因子大于6不可用
{
/*******************将当前惯导水平位置估计作为目标悬停点************************/
Total_Controler.Latitude_Position_Control.Expect=NamelessQuad.Position[_ROLL];
Total_Controler.Longitude_Position_Control.Expect=NamelessQuad.Position[_PITCH];
Pos_Hold_SetFlag=1;
}
}
/******若满足GPS定点模式，对Pos_Hold_SetFlag置1，允许进入定点模式*****************/
}

首先对于变量有定义：

#define  Deadband       300//油门中位死区
#define  Deadzone_Min   350//油门杆给定期望速度时，下行程临界值
#define  Deadzone_Max   650//油门杆给定期望速度时，上行程临界值
#define  Thr_Top 1000//油门最大上行程
#define  Thr_Buttom 0//油门最大下行程
#define  Climb_Up_Speed_Max    400//向上最大攀爬速度
#define  Climb_Down_Speed_Max  150//向下最大下降速度
#define  Thr_Start  1100//起转油门量
#define  Thr_Min 1000
#define  Thr_Idle 1100//油门怠速
uint16 Motor_PWM_1,Motor_PWM_2,Motor_PWM_3,Motor_PWM_4;//四个电机输出PWM
uint8 MotorTest=0xff;//电机序号测试

float Yaw_Infront_Feadback=0.35;//偏航角前馈控制;
uint8_t Controler_Mode=1,Last_Controler_Mode=1;
#define Self_Balance_Mode 1//自稳、纯姿态加油门补偿
#define High_Hold_Mode    2//定高模式
#define Pos_Hold_Mode 3//定点模式

uint8_t Control_Mode_Change=0;
#define Self_To_High  1//自稳切定高
#define High_To_Pos   2//定高切定点

//偏航模式
uint8 Yaw_Control_Mode=0;
#define No_Head_Mode 0
#define Guide_Direction_Mode 1

float  Position_Hold_Pitch=0,Position_Hold_Roll=0;
float  Speed_Hold_Pitch=0,Speed_Hold_Roll=0;
uint16_t High_Hold_Throttle=0;
uint8_t  High_Hold_SetFlag=0;
uint8_t  Pos_Hold_SetFlag=0;
uint16_t  HomePoint_Is_Okay=0;