0,0 → 1,76 |
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
{ |
tmpl3 = (MesswertRoll * winkel_nick) / 2048L; |
tmpl3 *= Parameter_AchsKopplung2; //65 |
tmpl3 /= 4096L; |
tmpl4 = (MesswertNick * winkel_roll) / 2048L; |
tmpl4 *= Parameter_AchsKopplung2; //65 |
tmpl4 /= 4096L; |
KopplungsteilNickRoll = tmpl3; |
KopplungsteilRollNick = tmpl4; |
tmpl4 -= tmpl3; |
ErsatzKompass += tmpl4; |
if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen |
|
tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
tmpl *= Parameter_AchsKopplung1; // 90 |
tmpl /= 4096L; |
tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
tmpl2 *= Parameter_AchsKopplung1; |
tmpl2 /= 4096L; |
if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
//MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
} |
else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
|
TrimRoll = tmpl - tmpl2 / 100L; |
TrimNick = -tmpl2 + tmpl / 100L; |
|
|
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mess_IntegralRoll2 += MesswertRoll + TrimRoll; |
Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll; |
if(Mess_IntegralRoll > Umschlag180Roll) |
{ |
Mess_IntegralRoll = -(Umschlag180Roll - 25000L); |
Mess_IntegralRoll2 = Mess_IntegralRoll; |
} |
if(Mess_IntegralRoll <-Umschlag180Roll) |
{ |
Mess_IntegralRoll = (Umschlag180Roll - 25000L); |
Mess_IntegralRoll2 = Mess_IntegralRoll; |
} |
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mess_IntegralNick2 += MesswertNick + TrimNick; |
Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick; |
if(Mess_IntegralNick > Umschlag180Nick) |
{ |
Mess_IntegralNick = -(Umschlag180Nick - 25000L); |
Mess_IntegralNick2 = Mess_IntegralNick; |
} |
if(Mess_IntegralNick <-Umschlag180Nick) |
{ |
Mess_IntegralNick = (Umschlag180Nick - 25000L); |
Mess_IntegralNick2 = Mess_IntegralNick; |
} |
|
|
.. |
|
if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
|
|
#define TRIM_MAX 200 |
if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
|
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / CONTROL_SCALING); |
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / CONTROL_SCALING); |
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / CONTROL_SCALING) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / CONTROL_SCALING)); |
|
messwertNick er nu P-part.... |
|