DebugOut.Analog[17] = IntegralAccNick / 26; DebugOut.Analog[18] = IntegralAccRoll / 26; DebugOut.Analog[19] = IntegralFehlerNick;// / 26; DebugOut.Analog[20] = IntegralFehlerRoll;// / 26; DebugOut.Analog[21] = MittelIntegralNick / 26; DebugOut.Analog[22] = MittelIntegralRoll / 26; //DebugOut.Analog[28] = ausgleichNick; DebugOut.Analog[29] = ausgleichRoll;
DebugOut.Analog[30] = LageKorrekturRoll * 10;
#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4) #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) #define BEWEGUNGS_LIMIT 20000
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
/*以下部分就是对LageKorrekturNick的修正和对陀螺仪常值误差的修正*/ cnt = 1;// + labs(IntegralFehlerNick) / 4096;
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT) {
if(IntegralFehlerNick > FEHLER_LIMIT2) {
/*必须连续两次的误差都很大，才能进入下面的if语句*/ if(last_n_p) {
/*连续两次误差较大时，对陀螺仪漂移进行补偿*/ /*最后改变了LageKorrekturNick的值*/
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; ausgleichNick = IntegralFehlerNick / 8; if(ausgleichNick > 5000) ausgleichNick = 5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; } else
last_n_p = 1; } else last_n_p = 0;
if(IntegralFehlerNick < -FEHLER_LIMIT2) {
if(last_n_n) {
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; ausgleichNick = IntegralFehlerNick / 8;
if(ausgleichNick < -5000) ausgleichNick = -5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; } else
last_n_n = 1; } else last_n_n = 0; }
else cnt = 0;
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
/*在飞行器飞行的过程中，如果发现陀螺仪的中立点发生变化，则仍然进行修正*/ /*误差过大时候，改变陀螺仪的常值误差，每次最多改变EE_Parameter.Driftkomp*/ if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ cnt = 1;// + labs(IntegralFehlerNick) / 4096;
ausgleichRoll = 0;
if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT) {
if(IntegralFehlerRoll > FEHLER_LIMIT2) {
if(last_r_p) {
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2; ausgleichRoll = IntegralFehlerRoll / 8; if(ausgleichRoll > 5000) ausgleichRoll = 5000;
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL; } else
last_r_p = 1; } else
last_r_p = 0;
if(IntegralFehlerRoll < -FEHLER_LIMIT2) { if(last_r_n) {
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2; ausgleichRoll = IntegralFehlerRoll / 8;
if(ausgleichRoll < -5000) ausgleichRoll = -5000;
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL; } else last_r_n = 1; } else last_r_n = 0; } else { cnt = 0; }
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
DebugOut.Analog[27] = ausgleichRoll;
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll); } // 整个的融合过程结束 else {
LageKorrekturRoll = 0; LageKorrekturNick = 0; }
/*如果IntegralFaktor为零，也就是没有使用陀螺仪积分对电机输出进行修正，则不使用LageKorrekturRoll，
也就是不进行陀螺仪漂移的补偿*/
/*在Heading_hold标志位被置位的情况下*/ if(!IntegralFaktor) {
LageKorrekturRoll = 0; LageKorrekturNick = 0; } // z.B. bei HH
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ /*将上一次的值储存下来*/
MittelIntegralNick_Alt = MittelIntegralNick; MittelIntegralRoll_Alt = MittelIntegralRoll;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ /*数据清零 加速度计积分每一次都进行清零*/ IntegralAccNick = 0; IntegralAccRoll = 0; IntegralAccZ = 0; MittelIntegralNick = 0; MittelIntegralRoll = 0; MittelIntegralNick2 = 0; MittelIntegralRoll2 = 0; ZaehlMessungen = 0;
} //end if of if(ZaehlMessungen >= ABGLEICH_ANZAHL) //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gieren//偏航
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(abs(StickGier) > 20) // war 35 {
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; }
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y
= ax + bx?
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; sollGier = tmp_int;
/*如果没有这句话 那么偏航轴的期望角度将一直等于0度 那么如果需要调整偏航轴的角度 就必须一直
不断的进行偏航的修正 加上这句话后 就不用一直修正了*/ Mess_Integral_Gier -= tmp_int;
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen约束和限制 if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+++++++++++++++++++++++++++++++++ // Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) { int w,v;
static int SignalSchlecht = 0; w = abs(IntegralNick /512); v = abs(IntegralRoll /512); if(v > w) w = v;
if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht) {
KompassStartwert = KompassValue; NeueKompassRichtungMerken = 0; }
w = (w * Parameter_KompassWirkung) / 64; w = Parameter_KompassWirkung - w; if(w > 0) {
if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; if(SignalSchlecht) SignalSchlecht--; }
else SignalSchlecht = 500; }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(!TimerWerteausgabe--) {
TimerWerteausgabe = 24;
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; DebugOut.Analog[2] = Mittelwert_AccNick; DebugOut.Analog[3] = Mittelwert_AccRoll; DebugOut.Analog[4] = MesswertGier; DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] =(Mess_Integral_Hoch / 512); DebugOut.Analog[8] = KompassValue;