| 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 / STICK_GAIN); |
| MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
| MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
| |
| messwertNick er nu P-part.... |
| |