无名飞控的姿态解算和控制
从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; // PitchRoll= 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;//+-180imuComputeRotationMatrix();
}算法可参照博客点击打开链接，这篇博客详细推导了基于梯度下降法和一阶毕卡法求解微分方程的姿态结算算法，最后给出了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;
接下来是一个四元数方程
/* 四元数微分方程计算本次待矫正四元数 */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; // PitchRoll= 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++;加一等于1if(Sample_Cnt==1){ Exti_Cnt++;Test_Cnt1=10000*TIME_ISR_CNT+TIM2->CNT/2;到现在为止过了多少msHC_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(Φ)^TR={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^2Origion_NamelessQuad.Acceleration[_PITCH]*=AcceGravity/AcceMax;Origion_NamelessQuad.Acceleration[_PITCH]*=100;//加速度cm/s^2Origion_NamelessQuad.Acceleration[_ROLL]*=AcceGravity/AcceMax;Origion_NamelessQuad.Acceleration[_ROLL]*=100;//加速度cm/s^2Acceleration_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];//沿地理坐标系，正东方向运动加速度,单位为CMSINS_Accel_Earth.y=Origion_NamelessQuad.Acceleration[_ROLL];//沿地理坐标系，正北方向运动加速度,单位为CMSINS_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_GY86Altitude_Estimate=AirPresure_Altitude;//高度观测量#elsedef IMU_BOARD_NC686Altitude_Estimate=SPL06_001_Filter_high;#elsedef IMU_BOARD_NC683Altitude_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;//单位msTime_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;//degLatitude=Ublox_Data[29]+(Ublox_Data[30]<<8)+(Ublox_Data[31]<<16)+(Ublox_Data[32]<<24);Latitude*=0.0000001f;//degLongitude_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 NGPS_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 EGPS_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 DGPS_Speed_Resolve[0]=GPS_Vel[_PITCH];//Y AxisGPS_Speed_Resolve[1]=GPS_Vel[_ROLL];//X AxisSpeed_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估计量的差，单位cmX_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_4uint32_t Continue_time,//持续时间/uint16_t Period,//周期100ms~1000msfloat 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 1float 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;
支付宝扫一扫
微信扫一扫
.png)
.png)
.png)
.png)
.png)
.png)
.png)
