Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1612 | dongfang | 1 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
| 2 | if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
||
| 3 | { |
||
| 4 | tmpl3 = (MesswertRoll * winkel_nick) / 2048L; |
||
| 5 | tmpl3 *= Parameter_AchsKopplung2; //65 |
||
| 6 | tmpl3 /= 4096L; |
||
| 7 | tmpl4 = (MesswertNick * winkel_roll) / 2048L; |
||
| 8 | tmpl4 *= Parameter_AchsKopplung2; //65 |
||
| 9 | tmpl4 /= 4096L; |
||
| 10 | KopplungsteilNickRoll = tmpl3; |
||
| 11 | KopplungsteilRollNick = tmpl4; |
||
| 12 | tmpl4 -= tmpl3; |
||
| 13 | ErsatzKompass += tmpl4; |
||
| 14 | if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen |
||
| 15 | |||
| 16 | tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
||
| 17 | tmpl *= Parameter_AchsKopplung1; // 90 |
||
| 18 | tmpl /= 4096L; |
||
| 19 | tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
||
| 20 | tmpl2 *= Parameter_AchsKopplung1; |
||
| 21 | tmpl2 /= 4096L; |
||
| 22 | if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
||
| 23 | //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
||
| 24 | } |
||
| 25 | else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
||
| 26 | |||
| 27 | TrimRoll = tmpl - tmpl2 / 100L; |
||
| 28 | TrimNick = -tmpl2 + tmpl / 100L; |
||
| 29 | |||
| 30 | |||
| 31 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
||
| 32 | Mess_IntegralRoll2 += MesswertRoll + TrimRoll; |
||
| 33 | Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll; |
||
| 34 | if(Mess_IntegralRoll > Umschlag180Roll) |
||
| 35 | { |
||
| 36 | Mess_IntegralRoll = -(Umschlag180Roll - 25000L); |
||
| 37 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
||
| 38 | } |
||
| 39 | if(Mess_IntegralRoll <-Umschlag180Roll) |
||
| 40 | { |
||
| 41 | Mess_IntegralRoll = (Umschlag180Roll - 25000L); |
||
| 42 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
||
| 43 | } |
||
| 44 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
||
| 45 | Mess_IntegralNick2 += MesswertNick + TrimNick; |
||
| 46 | Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick; |
||
| 47 | if(Mess_IntegralNick > Umschlag180Nick) |
||
| 48 | { |
||
| 49 | Mess_IntegralNick = -(Umschlag180Nick - 25000L); |
||
| 50 | Mess_IntegralNick2 = Mess_IntegralNick; |
||
| 51 | } |
||
| 52 | if(Mess_IntegralNick <-Umschlag180Nick) |
||
| 53 | { |
||
| 54 | Mess_IntegralNick = (Umschlag180Nick - 25000L); |
||
| 55 | Mess_IntegralNick2 = Mess_IntegralNick; |
||
| 56 | } |
||
| 57 | |||
| 58 | |||
| 59 | .. |
||
| 60 | |||
| 61 | if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
||
| 62 | else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
||
| 63 | if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
||
| 64 | else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
||
| 65 | |||
| 66 | |||
| 67 | #define TRIM_MAX 200 |
||
| 68 | if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
||
| 69 | if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
||
| 70 | |||
| 71 | MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
||
| 72 | MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
||
| 73 | MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
||
| 74 | |||
| 75 | messwertNick er nu P-part.... |
||
| 76 |