else {
if(Looping_Unten) // Hysterese {
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese)) Looping_Unten = 0; } }
/*不应该出现轴都是Looping的情况*/
if(Looping_Links || Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0;
if(Looping_Oben || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0;
Looping_Rechts = 0;} else Looping_Nick = 0; } // end if of if(!NewPpmData-- || Notlandung)
if(Looping_Roll) beeptime = 100; if(Looping_Roll || Looping_Nick) {
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(Notlandung) {
/*如果出现紧急降落，则将三个期望位置全部置零，即让飞行器向最稳定的方向调整，同时改变控制参
数，并且不让飞行器处在空中打转的状态*/ StickGier = 0; StickNick = 0; StickRoll = 0; GyroFaktor = 0.1; IntegralFaktor = 0.005; Looping_Roll = 0; Looping_Nick = 0; }
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+++++++++++++++++++++++++++++++++
// Integrale auf ACC-Signal abgleichen//加速度信号的积分校准
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #define ABGLEICH_ANZAHL 256L
/*计算陀螺仪积分的积分，为了和加速度计的积分做比较，进行角速率的补偿和陀螺仪中立点的修正*/ MittelIntegralNick += IntegralNick; MittelIntegralRoll += IntegralRoll; MittelIntegralNick2 += IntegralNick2; MittelIntegralRoll2 += IntegralRoll2;
/*在空中打转过程中，让所有的积分项都为零，因为机动过程会产生很大的误差，因此需要尽快结束其控制，然
后自动调平。 */
if(Looping_Nick || Looping_Roll) {
IntegralAccNick = 0; IntegralAccRoll = 0; MittelIntegralNick = 0; MittelIntegralRoll = 0; MittelIntegralNick2 = 0; MittelIntegralRoll2 = 0;
Mess_IntegralNick2 = Mess_IntegralNick; Mess_IntegralRoll2 = Mess_IntegralRoll; ZaehlMessungen = 0; LageKorrekturNick = 0; LageKorrekturRoll = 0; }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(!Looping_Nick && !Looping_Roll) {
long tmp_long, tmp_long2;
/*使用加速度计的值去补偿陀螺仪的积分，这里必须知道EE_Parameter.GyroAccFaktor参数，才能够知道补偿了 多少*/
/*其中IntegralNick应该是陀螺仪积分
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
是滤波后的加速度，用当前加速度和上次的加速度平均 */
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)
Mittelwert_AccNick);//
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)
Mittelwert_AccRoll); // tmp_long /= 16; tmp_long2 /= 16;
/*如果历史最大摇杆的量比较大，则说明在前段时间内飞行器的姿态可能不为0，这就导致加速度计的输出受到
有害加速度的影响，因此必须加速度计和陀螺仪积分差值的基础上做一次衰减*/ if((MaxStickNick > 15) || (MaxStickRoll > 15)) {
tmp_long /= 3; tmp_long2 /= 3; }
/*当偏航轴的操纵杆输入较大时候，则说明这时候偏航轴有一个角速度，为了消除有害加速度的影响，必须对这
两个数值再做一次衰减*/
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) {
tmp_long /= 3; tmp_long2 /= 3; }
/*做一个限制，补偿值必须在一定的范围内。将补偿的范围限制在+-32*/ #define AUSGLEICH 32 if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH;
/*将补偿值考虑进去，这时候Mess_IntegralNick补偿了，Mess_IntegralNick2没有补偿，因为在后面还要用到
*/
Mess_IntegralNick -= tmp_long; Mess_IntegralRoll -= tmp_long2;
} // end if of if(!Looping_Nick && !Looping_Roll)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*当 >ABGLEICH_ANZAHL(256)时候 说明测量了256次航向*/
/*变量ZaehlMessungen是在AD检测的函数中改变的，也就是说，下面这个if语句是每256个检测周期计算一次，
而不是控制周期，检测周期要高于控制周期*/
if(ZaehlMessungen >= ABGLEICH_ANZAHL)//关于时间积累的处理过程 {
static int cnt = 0;
static char last_n_p,last_n_n,last_r_p,last_r_n;
static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt; if(!Looping_Nick && !Looping_Roll) {
MittelIntegralNick /= ABGLEICH_ANZAHL; MittelIntegralRoll /= ABGLEICH_ANZAHL;
/*计算加速度计积分的作用，在不运动时候，xy加速度计的积分应该是0，所以xy积
分而z不积分*/
IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) /
ABGLEICH_ANZAHL;
IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) /
ABGLEICH_ANZAHL;
IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL; #define MAX_I 0//(Poti2/10)
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ /*不考虑补偿的陀螺仪积分的积分-加速度计积分/平衡项*/
IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick); ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll); ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in
[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) {
/*这个参数在后面的程序中还要进行修正，修正后的值加入到陀螺仪的积分
中，可以认为这个参数是系统对于陀螺仪漂移的估计*/ LageKorrekturNick /= 2; LageKorrekturRoll /= 2; }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gyro-Drift ermitteln//陀螺漂移的确定 /*对陀螺仪漂移的估计过程*/
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*前面MittelIntegralNick已经用过了，因此这里使用MittelIntegralNick2*/ MittelIntegralNick2 /= ABGLEICH_ANZAHL; MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
/*有校正和没有校正的陀螺仪积分做差，即陀螺仪的漂移*/
/*IntegralNick2是没有校正的陀螺仪积分 IntegralNick是有校正的陀螺仪积分 这里的校正指的是使用加速度
计积分进行的校正*/
tmp_long = IntegralNick2 - IntegralNick; tmp_long2 = IntegralRoll2 - IntegralRoll;
//DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
/*将差值加入到Mess_IntegralNick2和Mess_IntegralRoll2中 这时Mess_IntegralNick2和Mess_IntegralRoll2
被使用*/
IntegralFehlerNick = tmp_long; IntegralFehlerRoll = tmp_long2;
/*下面两个公式的作用就是让Mess_IntegralNick2=Mess_IntegralNick，
Mess_IntegralRoll2=Mess_IntegralRoll为下一个计算周期做准备*/ Mess_IntegralNick2 -= IntegralFehlerNick; Mess_IntegralRoll2 -= IntegralFehlerRoll;
// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;