Rev 2081 | Rev 2127 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2081 | Rev 2093 | ||
---|---|---|---|
Line 56... | Line 56... | ||
56 | 56 | ||
57 | #include "main.h" |
57 | #include "main.h" |
58 | #include "mymath.h" |
58 | #include "mymath.h" |
Line -... | Line 59... | ||
- | 59 | #include "isqrt.h" |
|
- | 60 | ||
- | 61 | ||
- | 62 | //MartinW; added vars |
|
- | 63 | unsigned char loop1, loop2, loop3; |
|
- | 64 | unsigned char settingdest = 5; |
|
- | 65 | char keynumber=-7; |
|
- | 66 | unsigned char jetistepstep[] = {1,5,10,25,50}, jetistep = 0; |
|
- | 67 | ||
- | 68 | ||
- | 69 | unsigned short CurrentOffset = 0;/// |
|
- | 70 | ||
- | 71 | unsigned char Motors[8]; |
|
- | 72 | unsigned char Motorsmax[8]; |
|
- | 73 | unsigned short MotorsTmax; |
|
- | 74 | unsigned char updatemotors=2; |
|
- | 75 | ||
- | 76 | //Panorama Trigger; |
|
- | 77 | int degreeold =0; |
|
- | 78 | int degreedivold =0; |
|
- | 79 | int degreediv =0; |
|
- | 80 | unsigned int panograd=0; |
|
- | 81 | unsigned char panotrigger=0; |
|
- | 82 | unsigned char calibration_done = 0; |
|
- | 83 | ||
- | 84 | unsigned char jetibeepcode[] = {130,'E','I','S','H'}; |
|
- | 85 | ||
- | 86 | ||
- | 87 | ///MartinW; added vars END |
|
- | 88 | ||
59 | #include "isqrt.h" |
89 | |
60 | 90 | ||
61 | unsigned char h,m,s; |
91 | unsigned char h,m,s; |
62 | unsigned int BaroExpandActive = 0; |
92 | unsigned int BaroExpandActive = 0; |
63 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
93 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
Line 72... | Line 102... | ||
72 | long IntegralRoll = 0,IntegralRoll2 = 0; |
102 | long IntegralRoll = 0,IntegralRoll2 = 0; |
73 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
103 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
74 | long Integral_Gier = 0; |
104 | long Integral_Gier = 0; |
75 | long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
105 | long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
76 | long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
106 | long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
77 | long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
107 | //long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; ///MartinW so war es |
- | 108 | long Mess_Integral_Gier = 0; ///MartinW: Mess_Integral_Gier2 unbenutzt |
|
- | 109 | ||
78 | long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
110 | long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
79 | long SummeNick=0,SummeRoll=0; |
111 | long SummeNick=0,SummeRoll=0; |
80 | volatile long Mess_Integral_Hoch = 0; |
112 | volatile long Mess_Integral_Hoch = 0; |
81 | int KompassValue = -1; |
113 | int KompassValue = -1; |
82 | int KompassSollWert = 0; |
114 | int KompassSollWert = 0; |
Line 111... | Line 143... | ||
111 | unsigned char CalibrationDone = 0; |
143 | unsigned char CalibrationDone = 0; |
112 | char NeueKompassRichtungMerken = 0; |
144 | char NeueKompassRichtungMerken = 0; |
113 | int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
145 | int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
114 | //float Ki = FAKTOR_I; |
146 | //float Ki = FAKTOR_I; |
115 | int Ki = 10300 / 33; |
147 | int Ki = 10300 / 33; |
- | 148 | int KiHH = 10300 / 33; // MartinR : für Ki bei HH über Schalter |
|
- | 149 | ||
116 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
150 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
117 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
151 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
Line 118... | Line 152... | ||
118 | 152 | ||
119 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
153 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
Line 162... | Line 196... | ||
162 | unsigned char Parameter_GlobalConfig; |
196 | unsigned char Parameter_GlobalConfig; |
163 | unsigned char Parameter_ExtraConfig; |
197 | unsigned char Parameter_ExtraConfig; |
164 | unsigned char Parameter_MaximumAltitude; |
198 | unsigned char Parameter_MaximumAltitude; |
165 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
199 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
166 | unsigned char CareFree = 0; |
200 | unsigned char CareFree = 0; |
167 | const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; // 15° steps |
201 | //const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; // 15° steps // MartinR: so war es |
- | 202 | const signed char sintab[62] = { 0, 2, 4, 6, 8, 10, 11, 13, 14, 15, 15, 16, 16, 16, 15, 14, 13, 11, 10, 8, 6, 4, 2, 0, -2, -4, -6, -8, -10, -11, -13, -14, -15, -15, -16, -16, -16, -15, -15, -14, -13, -11, -10, -8, -6, -4, -2, 0, 2, 4, 6, 8, 10, 11, 13, 14, 15, 15, 16, 16, 16}; // 7,5° steps //MartinR |
|
- | 203 | signed char cosinus, sinus; // MartinR : extern für PAN-Funktion |
|
Line 168... | Line 204... | ||
168 | 204 | ||
169 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
205 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
- | 206 | //int MaxStickNick = 0,MaxStickRoll = 0; MartinR: so war es |
|
- | 207 | int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0; // MartinR: stick_.._neutral hinzugefügt |
|
170 | int MaxStickNick = 0,MaxStickRoll = 0; |
208 | |
171 | unsigned int modell_fliegt = 0; |
209 | unsigned int modell_fliegt = 0; |
172 | volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0; |
210 | volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0; |
173 | long GIER_GRAD_FAKTOR = 1291; |
211 | long GIER_GRAD_FAKTOR = 1291; |
174 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
212 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
Line 199... | Line 237... | ||
199 | DebugOut.Analog[11] = ErsatzKompassInGrad; |
237 | DebugOut.Analog[11] = ErsatzKompassInGrad; |
200 | DebugOut.Analog[12] = Motor[0].SetPoint; |
238 | DebugOut.Analog[12] = Motor[0].SetPoint; |
201 | DebugOut.Analog[13] = Motor[1].SetPoint; |
239 | DebugOut.Analog[13] = Motor[1].SetPoint; |
202 | DebugOut.Analog[14] = Motor[2].SetPoint; |
240 | DebugOut.Analog[14] = Motor[2].SetPoint; |
203 | DebugOut.Analog[15] = Motor[3].SetPoint; |
241 | DebugOut.Analog[15] = Motor[3].SetPoint; |
- | 242 | ||
- | 243 | DebugOut.Analog[16] = cosinus; // MartinR: zur Einstellung der Pan-Funktion |
|
- | 244 | DebugOut.Analog[17] = sinus; // MartinR: test zur Einstellung der Pan-Funktion |
|
- | 245 | DebugOut.Analog[18] = ServoPanValue; // MartinR: zur Einstellung der Pan-Funktion |
|
- | 246 | DebugOut.Analog[19] = ServoRollValue; // MartinR: Test |
|
204 | DebugOut.Analog[20] = ServoNickValue; |
247 | DebugOut.Analog[20] = ServoNickValue; |
205 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
248 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
- | 249 | #ifdef WITH_REMAINCAPACITY // only include functions if DEBUG is defined in main.h |
|
- | 250 | ||
- | 251 | #warning : "### with REMAIN CAPACITY ###" |
|
- | 252 | DebugOut.Analog[23] = Capacity.RemainCapacity; |
|
- | 253 | #else |
|
206 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
254 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
- | 255 | #endif |
|
- | 256 | ||
207 | DebugOut.Analog[24] = SollHoehe/5; |
257 | DebugOut.Analog[24] = SollHoehe/5; |
208 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
258 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
209 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
259 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
210 | DebugOut.Analog[27] = KompassSollWert; |
260 | DebugOut.Analog[27] = KompassSollWert; |
211 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
261 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
Line 335... | Line 385... | ||
335 | Delay_ms_Mess(100); |
385 | Delay_ms_Mess(100); |
336 | Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick; |
386 | Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick; |
337 | Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll; |
387 | Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll; |
338 | IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
388 | IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
339 | IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
389 | IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
340 | Mess_IntegralNick = IntegralNick; |
390 | Mess_IntegralNick = IntegralNick; |
341 | Mess_IntegralRoll = IntegralRoll; |
391 | Mess_IntegralRoll = IntegralRoll; |
342 | Mess_Integral_Gier = 0; |
392 | Mess_Integral_Gier = 0; |
343 | StartLuftdruck = Luftdruck; |
393 | StartLuftdruck = Luftdruck; |
344 | VarioMeter = 0; |
394 | VarioMeter = 0; |
345 | Mess_Integral_Hoch = 0; |
395 | Mess_Integral_Hoch = 0; |
Line 372... | Line 422... | ||
372 | if((AdNeutralGier < 150 * 2) || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; }; |
422 | if((AdNeutralGier < 150 * 2) || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; }; |
373 | if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; }; |
423 | if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; }; |
374 | if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; }; |
424 | if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; }; |
375 | if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
425 | if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
376 | carefree_old = 70; |
426 | carefree_old = 70; |
377 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
427 | #if ((defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) && defined(WITH_HOTTMENU)) |
- | 428 | #warning : "### with Hottmenu ###" |
|
378 | LIBFC_HoTT_Clear(); |
429 | LIBFC_HoTT_Clear(); |
379 | #endif |
430 | #endif |
380 | } |
431 | } |
Line 387... | Line 438... | ||
387 | { |
438 | { |
388 | static signed long tmpl,tmpl2,tmpl3,tmpl4; |
439 | static signed long tmpl,tmpl2,tmpl3,tmpl4; |
389 | static signed int oldNick, oldRoll, d2Roll, d2Nick; |
440 | static signed int oldNick, oldRoll, d2Roll, d2Nick; |
390 | signed long winkel_nick, winkel_roll; |
441 | signed long winkel_nick, winkel_roll; |
391 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
442 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
392 | MesswertNick = (signed int) AdWertNickFilter / 8; |
443 | //MesswertNick = (signed int) AdWertNickFilter / 8; // MartinR: so war es |
393 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
444 | //MesswertRoll = (signed int) AdWertRollFilter / 8; // MartinR: so war es |
- | 445 | MesswertNick = (signed int) AdWertNickFilter ; // MartinR die Division /8 erfolgt bereits in der analog.c |
|
- | 446 | MesswertRoll = (signed int) AdWertRollFilter ; // MartinR die Division /8 erfolgt bereits in der analog.c |
|
394 | RohMesswertNick = MesswertNick; |
447 | RohMesswertNick = MesswertNick; |
395 | RohMesswertRoll = MesswertRoll; |
448 | RohMesswertRoll = MesswertRoll; |
Line 396... | Line 449... | ||
396 | 449 | ||
397 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
450 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 431... | Line 484... | ||
431 | tmpl4 *= Parameter_AchsKopplung2; //65 |
484 | tmpl4 *= Parameter_AchsKopplung2; //65 |
432 | tmpl4 /= 4096L; |
485 | tmpl4 /= 4096L; |
433 | KopplungsteilNickRoll = tmpl3; |
486 | KopplungsteilNickRoll = tmpl3; |
434 | KopplungsteilRollNick = tmpl4; |
487 | KopplungsteilRollNick = tmpl4; |
435 | tmpl4 -= tmpl3; |
488 | tmpl4 -= tmpl3; |
- | 489 | if(IntegralFaktor) // MartinR: nur im ACC-Mode |
|
- | 490 | { |
|
436 | ErsatzKompass += tmpl4; |
491 | ErsatzKompass += tmpl4; |
437 | if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen |
492 | if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen |
438 | 493 | } |
|
439 | tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
494 | tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
440 | tmpl *= Parameter_AchsKopplung1; // 90 |
495 | tmpl *= Parameter_AchsKopplung1; // 90 |
441 | tmpl /= 4096L; |
496 | tmpl /= 4096L; |
442 | tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
497 | tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
443 | tmpl2 *= Parameter_AchsKopplung1; |
498 | tmpl2 *= Parameter_AchsKopplung1; |
Line 482... | Line 537... | ||
482 | IntegralNick = Mess_IntegralNick; |
537 | IntegralNick = Mess_IntegralNick; |
483 | IntegralRoll = Mess_IntegralRoll; |
538 | IntegralRoll = Mess_IntegralRoll; |
484 | IntegralNick2 = Mess_IntegralNick2; |
539 | IntegralNick2 = Mess_IntegralNick2; |
485 | IntegralRoll2 = Mess_IntegralRoll2; |
540 | IntegralRoll2 = Mess_IntegralRoll2; |
Line -... | Line 541... | ||
- | 541 | ||
486 | 542 | //#define D_LIMIT 128 // MartinR: so war es |
|
- | 543 | #define D_LIMIT 16 |
|
487 | #define D_LIMIT 128 |
544 | // MartinR: Änderung war notwendig, da die Division /8 bereits in der analog.c erfolgt |
488 | 545 | ||
489 | MesswertNick = HiResNick / 8; |
546 | //MesswertNick = HiResNick / 8; // MartinR : so war es |
- | 547 | // MesswertRoll = HiResRoll / 8; // MartinR : so war es |
|
- | 548 | MesswertNick = HiResNick ; // MartinR die Division /8 erfolgt bereits in der analog.c |
|
Line -... | Line 549... | ||
- | 549 | MesswertRoll = HiResRoll ; // MartinR die Division /8 erfolgt bereits in der analog.c |
|
- | 550 | ||
- | 551 | // MartinR : so war es Anfang |
|
490 | MesswertRoll = HiResRoll / 8; |
552 | /* |
491 | 553 | |
|
492 | if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000; |
554 | if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000; |
493 | if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; } |
555 | if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; } |
494 | else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; } |
556 | else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; } |
495 | if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000; |
557 | if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000; |
- | 558 | if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; } |
|
- | 559 | else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; } |
|
- | 560 | |
|
- | 561 | // MartinR : FC 1.0: Sprung von 500 auf 2000 !! FC-ME: Sprung von 1000 auf 2000 |
|
- | 562 | */ |
|
- | 563 | // MartinR : so war es Ende |
|
- | 564 | ||
- | 565 | // MartinR : Neu Anfang |
|
- | 566 | if(PlatinenVersion == 10) |
|
- | 567 | { |
|
- | 568 | if(AdWertNick > 1010) MesswertNick = +600; |
|
- | 569 | if(AdWertNick > 1017) MesswertNick = +800; |
|
- | 570 | if(AdWertNick < 15) MesswertNick = -600; |
|
- | 571 | if(AdWertNick < 7) MesswertNick = -800; |
|
- | 572 | if(AdWertRoll > 1010) MesswertRoll = +600; |
|
- | 573 | if(AdWertRoll > 1017) MesswertRoll = +800; |
|
- | 574 | if(AdWertRoll < 15) MesswertRoll = -600; |
|
- | 575 | if(AdWertRoll < 7) MesswertRoll = -800; |
|
- | 576 | } |
|
- | 577 | else |
|
- | 578 | { |
|
- | 579 | if(AdWertNick > 2000) MesswertNick = +1200; |
|
- | 580 | if(AdWertNick > 2015) MesswertNick = +1600; |
|
- | 581 | if(AdWertNick < 15) MesswertNick = -1200; |
|
- | 582 | if(AdWertNick < 7) MesswertNick = -1600; |
|
- | 583 | if(AdWertRoll > 2000) MesswertRoll = +1200; |
|
- | 584 | if(AdWertRoll > 2015) MesswertRoll = +1600; |
|
- | 585 | if(AdWertRoll < 15) MesswertRoll = -1200; |
|
- | 586 | if(AdWertRoll < 7) MesswertRoll = -1600; |
|
Line 496... | Line 587... | ||
496 | if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; } |
587 | } |
- | 588 | // MartinR : Neu Ende |
|
- | 589 | ||
497 | else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; } |
590 | if(Parameter_Gyro_D) |
498 | 591 | // MartinR: hier sind Änderungen erforderlich, da u.a. MesswertNick = HiResNick / 8 von der fc.c in die analog.c verschoben wurde |
|
499 | if(Parameter_Gyro_D) |
592 | // Hintergrund: Code einsparen |
500 | { |
593 | { |
501 | d2Nick = HiResNick - oldNick; |
594 | d2Nick = HiResNick - oldNick; |
Line 502... | Line 595... | ||
502 | oldNick = (oldNick + HiResNick)/2; |
595 | oldNick = (oldNick + HiResNick)/2; |
503 | if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
596 | if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
504 | else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
597 | else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
505 | 598 | ||
506 | d2Roll = HiResRoll - oldRoll; |
599 | d2Roll = HiResRoll - oldRoll; |
507 | oldRoll = (oldRoll + HiResRoll)/2; |
600 | oldRoll = (oldRoll + HiResRoll)/2; |
- | 601 | if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
|
508 | if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
602 | else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
- | 603 | ||
509 | else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
604 | //MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; // MartinR : so war es |
510 | 605 | MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 2; // MartinR : geändert |
|
511 | MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; |
606 | //MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; // MartinR : so war es |
Line 512... | Line 607... | ||
512 | MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; |
607 | MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 2; // MartinR : geändert |
513 | HiResNick += (d2Nick * (signed int) Parameter_Gyro_D); |
608 | HiResNick += (d2Nick * (signed int) Parameter_Gyro_D * 8); // martinR: *8 hinzugefügt |
514 | HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D); |
609 | HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D * 8); // martinR: *8 hinzugefügt |
Line 630... | Line 725... | ||
630 | Parameter_ExtraConfig = EE_Parameter.ExtraConfig; |
725 | Parameter_ExtraConfig = EE_Parameter.ExtraConfig; |
631 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
726 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
632 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
727 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
633 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
728 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
634 | Ki = 10300 / (Parameter_I_Faktor + 1); |
729 | Ki = 10300 / (Parameter_I_Faktor + 1); |
- | 730 | //Ki = (10300 / (Parameter_I_Faktor + 4)) + (StickGas /2); // MartinR: Test Gasabhängige Regelung |
|
- | 731 | ||
- | 732 | if(Parameter_UserParam1 > 50) KiHH = 10300 / (Parameter_UserParam2 + 1); else KiHH = Ki; // MartinR : für HH über Schalter |
|
- | 733 | //if(Parameter_UserParam1 > 50) KiHH = (10300 / (Parameter_UserParam2 + 4)) + (StickGas /2); else KiHH = Ki; // MartinR : für HH über Schalter // MartinR: Test Gasabhängige Regelung |
|
- | 734 | Parameter_NaviGpsModeControl = EE_Parameter.NaviGpsModeControl; //MartinR: Standard: EE_Parameter.NaviGpsModeControl wird übertragen |
|
- | 735 | if(!IntegralFaktor) Parameter_NaviGpsModeControl= 0; // MartinR: wenn HH dann GPS auf free- Mode |
|
- | 736 | // 0 = free; 100 = AID; 200 = coming home //neu |
|
- | 737 | ||
- | 738 | ||
635 | MAX_GAS = EE_Parameter.Gas_Max; |
739 | MAX_GAS = EE_Parameter.Gas_Max; |
636 | MIN_GAS = EE_Parameter.Gas_Min; |
740 | MIN_GAS = EE_Parameter.Gas_Min; |
Line 637... | Line 741... | ||
637 | 741 | ||
638 | tmp = EE_Parameter.CareFreeModeControl; |
742 | tmp = EE_Parameter.CareFreeModeControl; |
Line 672... | Line 776... | ||
672 | void MotorRegler(void) |
776 | void MotorRegler(void) |
673 | //############################################################################ |
777 | //############################################################################ |
674 | { |
778 | { |
675 | int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2; |
779 | int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2; |
676 | int GierMischanteil,GasMischanteil; |
780 | int GierMischanteil,GasMischanteil; |
- | 781 | ||
- | 782 | static long SummeNickHH=0,SummeRollHH=0; // MartinR: hinzugefügt |
|
- | 783 | ||
677 | static long sollGier = 0,tmp_long,tmp_long2; |
784 | static long sollGier = 0,tmp_long,tmp_long2; |
678 | static long IntegralFehlerNick = 0; |
785 | static long IntegralFehlerNick = 0; |
679 | static long IntegralFehlerRoll = 0; |
786 | static long IntegralFehlerRoll = 0; |
680 | static unsigned int RcLostTimer; |
787 | static unsigned int RcLostTimer; |
681 | static unsigned char delay_neutral = 0; |
788 | static unsigned char delay_neutral = 0; |
Line 786... | Line 893... | ||
786 | ServoActive = 0; |
893 | ServoActive = 0; |
787 | SetNeutral(0); |
894 | SetNeutral(0); |
788 | CalibrationDone = 1; |
895 | CalibrationDone = 1; |
789 | ServoActive = 1; |
896 | ServoActive = 1; |
790 | DDRD |=0x80; // enable J7 -> Servo signal |
897 | DDRD |=0x80; // enable J7 -> Servo signal |
- | 898 | JetiBeep = jetibeepcode[GetActiveParamSet()-1]; |
|
791 | Piep(GetActiveParamSet(),120); |
899 | Piep(GetActiveParamSet(),120); |
- | 900 | PPM_in[13] = Parameter_UserParam5 -127 ; // MartinR: Initialisierungswerte für die seriellen Potis für Jeti+ |
|
- | 901 | PPM_in[14] = Parameter_UserParam6 -127 ; // MartinR: Initialisierungswerte für die seriellen Potis für Jeti+ |
|
792 | } |
902 | } |
793 | } |
903 | } |
794 | } |
904 | } |
795 | else |
905 | else |
796 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
906 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
Line 828... | Line 938... | ||
828 | { |
938 | { |
829 | modell_fliegt = 1; |
939 | modell_fliegt = 1; |
830 | MotorenEin = 1; |
940 | MotorenEin = 1; |
831 | sollGier = 0; |
941 | sollGier = 0; |
832 | Mess_Integral_Gier = 0; |
942 | Mess_Integral_Gier = 0; |
833 | Mess_Integral_Gier2 = 0; |
943 | // Mess_Integral_Gier2 = 0; |
834 | Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
944 | Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
835 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
945 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
836 | Mess_IntegralNick2 = IntegralNick; |
946 | Mess_IntegralNick2 = IntegralNick; |
837 | Mess_IntegralRoll2 = IntegralRoll; |
947 | Mess_IntegralRoll2 = IntegralRoll; |
838 | SummeNick = 0; |
948 | SummeNick = 0; |
Line 875... | Line 985... | ||
875 | { |
985 | { |
876 | static int stick_nick,stick_roll; |
986 | static int stick_nick,stick_roll; |
877 | unsigned char stick_p; |
987 | unsigned char stick_p; |
878 | ParameterZuordnung(); |
988 | ParameterZuordnung(); |
879 | stick_p = EE_Parameter.Stick_P; |
989 | stick_p = EE_Parameter.Stick_P; |
- | 990 | // MartinR: original: |
|
- | 991 | /* |
|
880 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4; |
992 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4; |
881 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
993 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
882 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4; |
994 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4; |
883 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
995 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
- | 996 | */ |
|
- | 997 | // MartinR: geändert Anfang |
|
- | 998 | if(Parameter_UserParam1 > 50) // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist |
|
- | 999 | { |
|
- | 1000 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral) / 4; |
|
- | 1001 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral) / 4 ; |
|
- | 1002 | //stick_nick = (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral); |
|
- | 1003 | //stick_roll = (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral); |
|
- | 1004 | } |
|
- | 1005 | ||
- | 1006 | else |
|
- | 1007 | { |
|
- | 1008 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4; |
|
- | 1009 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4; |
|
- | 1010 | stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
|
- | 1011 | stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
|
- | 1012 | } |
|
- | 1013 | ||
- | 1014 | if(IntegralFaktor) |
|
- | 1015 | { |
|
- | 1016 | //stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
|
- | 1017 | //stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
|
- | 1018 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
|
- | 1019 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
|
- | 1020 | ||
- | 1021 | //StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam |
|
- | 1022 | //StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam |
|
- | 1023 | } |
|
- | 1024 | /*else // wenn HH , MartinR |
|
- | 1025 | { |
|
- | 1026 | //stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben |
|
- | 1027 | //stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben |
|
- | 1028 | //StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam |
|
- | 1029 | //StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam |
|
- | 1030 | } |
|
- | 1031 | */ |
|
- | 1032 | // MartinR: geändert Ende |
|
Line 884... | Line 1033... | ||
884 | 1033 | ||
885 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1034 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
886 | // CareFree und freie Wahl der vorderen Richtung |
1035 | // CareFree und freie Wahl der vorderen Richtung |
887 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1036 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 1037 | //if(CareFree) // MartinR: so war es |
|
888 | if(CareFree) |
1038 | if(CareFree && IntegralFaktor) // MartinR: CareFree nur im ACC-Mode |
889 | { |
1039 | { |
890 | signed int nick, roll; |
1040 | signed int nick, roll; |
891 | nick = stick_nick / 4; |
1041 | nick = stick_nick / 4; |
892 | roll = stick_roll / 4; |
1042 | roll = stick_roll / 4; |
893 | StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4); |
1043 | StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4); |
894 | StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4); |
1044 | StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4); |
895 | } |
1045 | } |
896 | else |
1046 | else |
897 | { |
1047 | { |
- | 1048 | //FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6]; //MartinR: so war es |
|
898 | FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6]; |
1049 | FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle * 2 + 12]; //MartinR: feinere Auflösung |
- | 1050 | //FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; //MartinR: so war es |
|
899 | FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; |
1051 | FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle * 2]; //MartinR: feinere Auflösung |
900 | StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8; |
1052 | StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8; |
901 | StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8; |
1053 | StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8; |
Line 902... | Line 1054... | ||
902 | } |
1054 | } |
Line 921... | Line 1073... | ||
921 | IntegralFaktorGier = Parameter_Gyro_Gier_I; |
1073 | IntegralFaktorGier = Parameter_Gyro_Gier_I; |
Line 922... | Line 1074... | ||
922 | 1074 | ||
923 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1075 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
924 | //+ Analoge Steuerung per Seriell |
1076 | //+ Analoge Steuerung per Seriell |
- | 1077 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 1078 | #ifdef WITH_ExternControl /// MartinW memorysaving |
|
- | 1079 | #warning : "### with ExternControl ###" |
|
925 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1080 | |
926 | if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) |
1081 | if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) |
927 | { |
1082 | { |
928 | StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
1083 | StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
929 | StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; |
1084 | StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; |
Line 946... | Line 1101... | ||
946 | MaxStickRoll = abs(StickRoll)/STICK_GAIN; |
1101 | MaxStickRoll = abs(StickRoll)/STICK_GAIN; |
947 | if(MaxStickRoll > 100) MaxStickRoll = 100; |
1102 | if(MaxStickRoll > 100) MaxStickRoll = 100; |
948 | } |
1103 | } |
949 | else MaxStickRoll--; |
1104 | else MaxStickRoll--; |
950 | if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING) {MaxStickNick = 0; MaxStickRoll = 0;} |
1105 | if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING) {MaxStickNick = 0; MaxStickRoll = 0;} |
- | 1106 | ||
- | 1107 | #else |
|
- | 1108 | #warning : "### without ExternControl ###" |
|
- | 1109 | #endif |
|
Line 951... | Line 1110... | ||
951 | 1110 | ||
952 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1111 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
953 | // Looping? |
1112 | // Looping? |
954 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1113 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1032... | Line 1191... | ||
1032 | MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren |
1191 | MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren |
1033 | MittelIntegralRoll += IntegralRoll; |
1192 | MittelIntegralRoll += IntegralRoll; |
1034 | MittelIntegralNick2 += IntegralNick2; |
1193 | MittelIntegralNick2 += IntegralNick2; |
1035 | MittelIntegralRoll2 += IntegralRoll2; |
1194 | MittelIntegralRoll2 += IntegralRoll2; |
Line 1036... | Line 1195... | ||
1036 | 1195 | ||
- | 1196 | // if(Looping_Nick || Looping_Roll) // MartinR: so war es |
|
1037 | if(Looping_Nick || Looping_Roll) |
1197 | if(Looping_Nick || Looping_Roll || (!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert |
1038 | { |
1198 | { |
1039 | IntegralAccNick = 0; |
1199 | IntegralAccNick = 0; |
1040 | IntegralAccRoll = 0; |
1200 | IntegralAccRoll = 0; |
1041 | MittelIntegralNick = 0; |
1201 | MittelIntegralNick = 0; |
Line 1046... | Line 1206... | ||
1046 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
1206 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
1047 | ZaehlMessungen = 0; |
1207 | ZaehlMessungen = 0; |
1048 | LageKorrekturNick = 0; |
1208 | LageKorrekturNick = 0; |
1049 | LageKorrekturRoll = 0; |
1209 | LageKorrekturRoll = 0; |
1050 | } |
1210 | } |
- | 1211 | ||
- | 1212 | if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) ) // MartinR: |
|
- | 1213 | // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH |
|
- | 1214 | // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten! |
|
- | 1215 | // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet |
|
- | 1216 | { |
|
- | 1217 | ||
- | 1218 | IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
|
- | 1219 | IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
|
- | 1220 | Mess_IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
|
- | 1221 | Mess_IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
|
- | 1222 | Mess_Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
|
- | 1223 | Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
|
- | 1224 | //Mess_Integral_Gier2 = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
|
- | 1225 | KompassSollWert = KompassValue; // MartinR: aktuelle Ausrichtung beibehalten |
|
- | 1226 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // MartinR: aktuelle Ausrichtung beibehalten |
|
- | 1227 | } |
|
- | 1228 | ||
- | 1229 | if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // MartinR geändert und verschoben |
|
- | 1230 | else IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen |
|
Line 1051... | Line 1231... | ||
1051 | 1231 | ||
1052 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1232 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1053 | if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
1233 | if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
1054 | { |
1234 | { |
Line 1105... | Line 1285... | ||
1105 | if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
1285 | if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
1106 | { |
1286 | { |
1107 | static int cnt = 0; |
1287 | static int cnt = 0; |
1108 | static char last_n_p,last_n_n,last_r_p,last_r_n; |
1288 | static char last_n_p,last_n_n,last_r_p,last_r_n; |
1109 | static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt; |
1289 | static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt; |
1110 | if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp) |
1290 | //if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp) // MartinR: so war es |
- | 1291 | if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt |
|
- | 1292 | ||
1111 | { |
1293 | { |
1112 | MittelIntegralNick /= ABGLEICH_ANZAHL; |
1294 | MittelIntegralNick /= ABGLEICH_ANZAHL; |
1113 | MittelIntegralRoll /= ABGLEICH_ANZAHL; |
1295 | MittelIntegralRoll /= ABGLEICH_ANZAHL; |
1114 | IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
1296 | IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
1115 | IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
1297 | IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
Line 1265... | Line 1447... | ||
1265 | } |
1447 | } |
1266 | tmp_int = (long) EE_Parameter.StickGier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
1448 | tmp_int = (long) EE_Parameter.StickGier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
1267 | tmp_int += (EE_Parameter.StickGier_P * StickGier) / 4; |
1449 | tmp_int += (EE_Parameter.StickGier_P * StickGier) / 4; |
1268 | tmp_int += CompassGierSetpoint; |
1450 | tmp_int += CompassGierSetpoint; |
1269 | sollGier = tmp_int; |
1451 | sollGier = tmp_int; |
1270 | Mess_Integral_Gier -= tmp_int; |
1452 | //Mess_Integral_Gier -= tmp_int; // MartinR: so war es |
- | 1453 | Mess_Integral_Gier -= (tmp_int * 10) / 8; // MartinR: Test um Zurückschwingen bei hohen I-Faktoren zu verringern |
|
1271 | if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
1454 | //if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen // MartinR: so war es |
1272 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
1455 | //if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; // MartinR: so war es |
- | 1456 | if(Mess_Integral_Gier > 90000) Mess_Integral_Gier = 90000; // begrenzen // MartinR: Begrenzung verändert |
|
- | 1457 | if(Mess_Integral_Gier <-90000) Mess_Integral_Gier =-90000; // MartinR: Begrenzung verändert |
|
Line 1273... | Line 1458... | ||
1273 | 1458 | ||
1274 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1459 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1275 | // Kompass |
1460 | // Kompass |
1276 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1461 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 1462 | //if(KompassValue >= 0 && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) // MartinR: so war es |
|
1277 | if(KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV)) |
1463 | if((KompassValue >= 0 && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) && !(Parameter_UserParam1 > 50)) // MartinR: bei HH über Schalter wird der Kompass abgeschaltet |
1278 | { |
1464 | { |
1279 | if(CalculateCompassTimer-- == 1) |
1465 | if(CalculateCompassTimer-- == 1) |
1280 | { |
1466 | { |
1281 | int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt |
1467 | int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt |
Line 1329... | Line 1515... | ||
1329 | 1515 | ||
1330 | #define TRIM_MAX 200 |
1516 | #define TRIM_MAX 200 |
1331 | if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
1517 | if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
Line -... | Line 1518... | ||
- | 1518 | if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
|
- | 1519 | ||
- | 1520 | //MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);// MartinR so war es |
|
- | 1521 | //MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);// MartinR so war es |
|
- | 1522 | ||
- | 1523 | if(!IntegralFaktor) // MartinR : HH-Mode hinzugefügt |
|
- | 1524 | { |
|
- | 1525 | MesswertNick = (long) ((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugefügt |
|
- | 1526 | MesswertRoll = (long) ((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugefügt |
|
- | 1527 | //MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN); |
|
- | 1528 | //Mess_Integral_Gier = 0; // MartinR: nur Kompass wird bei HH deaktiviert |
|
- | 1529 | //Integral_Gier = 0; // MartinR: nur Kompass wird bei HH deaktiviert |
|
- | 1530 | } |
|
1332 | if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
1531 | else // MartinR: ACC-Mode so war es |
1333 | 1532 | { |
|
- | 1533 | MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
|
- | 1534 | MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
|
1334 | MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
1535 | } |
Line 1335... | Line 1536... | ||
1335 | MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
1536 | |
1336 | MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
1537 | MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
1337 | 1538 | ||
Line 1420... | Line 1621... | ||
1420 | } |
1621 | } |
Line 1421... | Line 1622... | ||
1421 | 1622 | ||
1422 | // if height control is activated by an rc channel |
1623 | // if height control is activated by an rc channel |
1423 | if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1624 | if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1424 | { // check if parameter is less than activation threshold |
1625 | { // check if parameter is less than activation threshold |
- | 1626 | // if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position // MartinR :so war es |
|
1425 | if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position |
1627 | if((Parameter_HoehenSchalter < 50) || (Parameter_UserParam1 > 140) ) // MartinR: Schalter aus oder HH ohne Höhenregler über UsererParam1 an |
1426 | { //height control not active |
1628 | { //height control not active |
1427 | if(!delay--) |
1629 | if(!delay--) |
1428 | { |
1630 | { |
1429 | HoehenReglerAktiv = 0; // disable height control |
1631 | HoehenReglerAktiv = 0; // disable height control |
Line 1438... | Line 1640... | ||
1438 | } |
1640 | } |
1439 | } |
1641 | } |
1440 | else // no switchable height control |
1642 | else // no switchable height control |
1441 | { |
1643 | { |
1442 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_HoehenSchalter) * (int)EE_Parameter.Hoehe_Verstaerkung; |
1644 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_HoehenSchalter) * (int)EE_Parameter.Hoehe_Verstaerkung; |
- | 1645 | // HoehenReglerAktiv = 1; // MartinR : so war es |
|
- | 1646 | // MartinR : geändert Anfang |
|
- | 1647 | if(Parameter_UserParam1 > 140) // HH über Schalter: HH an + Höhenregler abgeschaltet, Nachführen von Parametern |
|
- | 1648 | { |
|
- | 1649 | HoehenReglerAktiv = 0; |
|
- | 1650 | } |
|
- | 1651 | else // Höhenregler mit Sollhöhe über Poti aktiv |
|
- | 1652 | { |
|
1443 | HoehenReglerAktiv = 1; |
1653 | HoehenReglerAktiv = 1; |
- | 1654 | } |
|
- | 1655 | // MartinR : geändert Ende |
|
1444 | } |
1656 | } |
Line 1445... | Line 1657... | ||
1445 | 1657 | ||
1446 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1658 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1447 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
1659 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
Line 1472... | Line 1684... | ||
1472 | { |
1684 | { |
1473 | // alternative height control |
1685 | // alternative height control |
1474 | // PD-Control with respect to hoover point |
1686 | // PD-Control with respect to hoover point |
1475 | // the thrust loss out of horizontal attitude is compensated |
1687 | // the thrust loss out of horizontal attitude is compensated |
1476 | // the setpoint will be fine adjusted with the gas stick position |
1688 | // the setpoint will be fine adjusted with the gas stick position |
- | 1689 | HeightDeviation = HoehenWert - SollHoehe; //MartinR: Test |
|
1477 | if(FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying |
1690 | if(FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying |
1478 | { // gas stick is above hoover point |
1691 | { // gas stick is above hoover point |
1479 | if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1692 | if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1480 | { |
1693 | { |
1481 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) |
1694 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) |
1482 | { |
1695 | { |
1483 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
1696 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
1484 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1697 | //SollHoehe = HoehenWert; // update setpoint to current heigth // MartinR: so war es |
- | 1698 | SollHoehe = HoehenWert - HeightDeviation / 2; // MartinR: um den Sollwertsprung zu verringern |
|
1485 | } |
1699 | } |
1486 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
1700 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
1487 | // Limit the maximum Altitude |
1701 | // Limit the maximum Altitude |
1488 | if(Parameter_MaximumAltitude && (SollHoehe/100 > Parameter_MaximumAltitude)) AltitudeSetpointTrimming = 0; |
1702 | if(Parameter_MaximumAltitude && (SollHoehe/100 > Parameter_MaximumAltitude)) AltitudeSetpointTrimming = 0; |
1489 | else |
1703 | else |
Line 1498... | Line 1712... | ||
1498 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
1712 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
1499 | { |
1713 | { |
1500 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) |
1714 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) |
1501 | { |
1715 | { |
1502 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
1716 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
1503 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1717 | //SollHoehe = HoehenWert; // update setpoint to current heigth // MartinR: so war es |
- | 1718 | SollHoehe = HoehenWert - HeightDeviation / 2; // MartinR: um den Sollwertsprung zu verringern |
|
1504 | } |
1719 | } |
1505 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
1720 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
1506 | AltitudeSetpointTrimming = -abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1721 | AltitudeSetpointTrimming = -abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1507 | // HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1722 | // HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1508 | VarioCharacter = '-'; |
1723 | VarioCharacter = '-'; |
Line 1519... | Line 1734... | ||
1519 | WaypointTrimming = 10; |
1734 | WaypointTrimming = 10; |
1520 | VarioCharacter = '^'; |
1735 | VarioCharacter = '^'; |
1521 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) // changed from sinking to rising |
1736 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) // changed from sinking to rising |
1522 | { |
1737 | { |
1523 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
1738 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
1524 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1739 | //SollHoehe = HoehenWert; // update setpoint to current heigth // MartinR: so war es |
- | 1740 | SollHoehe = HoehenWert - HeightDeviation / 2; // MartinR: um den Sollwertsprung zu verringern |
|
1525 | } |
1741 | } |
1526 | } |
1742 | } |
1527 | else |
1743 | else |
1528 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint < SollHoehe) // von NC gesteuert -> sinken |
1744 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint < SollHoehe) // von NC gesteuert -> sinken |
1529 | { |
1745 | { |
Line 1533... | Line 1749... | ||
1533 | WaypointTrimming = -10; |
1749 | WaypointTrimming = -10; |
1534 | VarioCharacter = 'v'; |
1750 | VarioCharacter = 'v'; |
1535 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) // changed from rising to sinking |
1751 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) // changed from rising to sinking |
1536 | { |
1752 | { |
1537 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
1753 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
1538 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1754 | //SollHoehe = HoehenWert; // update setpoint to current heigth // MartinR: so war es |
- | 1755 | SollHoehe = HoehenWert - HeightDeviation / 2; // MartinR: um den Sollwertsprung zu verringern |
|
1539 | } |
1756 | } |
1540 | } |
1757 | } |
1541 | else |
1758 | else |
1542 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
1759 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
1543 | { |
1760 | { |
Line 1602... | Line 1819... | ||
1602 | { |
1819 | { |
1603 | // ------------------------- P-Part ---------------------------- |
1820 | // ------------------------- P-Part ---------------------------- |
1604 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
1821 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
1605 | LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
1822 | LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
1606 | HeightDeviation = (int)(tmp_long); // positive when too high |
1823 | HeightDeviation = (int)(tmp_long); // positive when too high |
- | 1824 | //tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part // MartinR: so war es |
|
- | 1825 | // MartinR: geändert Anfang |
|
- | 1826 | if ((SollHoehe > (HoehenWert+64)) || (SollHoehe < (HoehenWert-64))) |
|
- | 1827 | { |
|
- | 1828 | //tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 24L; // p-part // MartinR P-Part erhoehen |
|
- | 1829 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 70L; // p-part // MartinR Anpassung an Standardwert |
|
- | 1830 | } |
|
- | 1831 | else |
|
- | 1832 | { |
|
1607 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
1833 | //tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
- | 1834 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 90L; // MartinR Anpassung an Standardwert |
|
- | 1835 | } |
|
- | 1836 | // MartinR: geändert Ende |
|
1608 | LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense |
1837 | //LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense // MartinR: so war es |
- | 1838 | // MartinR: weshalb unsymmetrisch? |
|
- | 1839 | LIMIT_MIN_MAX(tmp_long, -128 * STICK_GAIN, 128 * STICK_GAIN); // more than the full range makes no sense // MartinR: geändert |
|
1609 | GasReduction = tmp_long; |
1840 | GasReduction = tmp_long; |
1610 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1841 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1611 | tmp_int = VarioMeter / 8; |
1842 | //tmp_int = VarioMeter / 8; // MartinR: so war es |
- | 1843 | // MartinR: geändert Anfang |
|
- | 1844 | tmp_int = VarioMeter / 4; // MartinR: geändert // Variometer: steigen ist positiv |
|
- | 1845 | { |
|
- | 1846 | if ((SollHoehe > (HoehenWert+512)) || (SollHoehe < (HoehenWert-512))) |
|
- | 1847 | //if ((StickGas > (StickGasHover + 3*HEIGHT_CONTROL_STICKTHRESHOLD)) || (StickGas < (StickGasHover - 3*HEIGHT_CONTROL_STICKTHRESHOLD))) |
|
- | 1848 | { |
|
- | 1849 | tmp_int = tmp_int + HeightDeviation / 28; |
|
- | 1850 | //tmp_int = tmp_int + Parameter_Hoehe_ACC_Wirkung* HeightDeviation / 64; // reduce d-part while trimming setpoint // MartinR: geändert |
|
- | 1851 | } |
|
- | 1852 | else |
|
- | 1853 | { |
|
- | 1854 | tmp_int = tmp_int + HeightDeviation / 32; |
|
- | 1855 | //tmp_int = tmp_int + Parameter_Hoehe_ACC_Wirkung* HeightDeviation / 64; |
|
- | 1856 | } |
|
- | 1857 | } |
|
- | 1858 | // MartinR: geändert Ende |
|
- | 1859 | ||
1612 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
1860 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
1613 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
1861 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
1614 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
1862 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
- | 1863 | /* // MartinR: so war es Anfang |
|
1615 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint |
1864 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint |
1616 | else |
1865 | else |
1617 | if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
1866 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
- | 1867 | */ // MartinR: so war es Ende |
|
- | 1868 | tmp_int /= 4; // MartinR: geändert: keine veränderung des d-part im "Deckel" mode |
|
- | 1869 | //if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint // MartinR: geändert |
|
- | 1870 | ||
1618 | GasReduction += tmp_int; |
1871 | GasReduction += tmp_int; |
1619 | } // EOF no baro range expanding |
1872 | } // EOF no baro range expanding |
1620 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1873 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
- | 1874 | /* // MartinR: deaktiviert Anfang, da statische Ablage bei Schräglage Probleme macht |
|
1621 | if(Parameter_Hoehe_ACC_Wirkung) |
1875 | if(Parameter_Hoehe_ACC_Wirkung) |
1622 | { |
1876 | { |
1623 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1877 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1624 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1878 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1625 | GasReduction += tmp_long; |
1879 | GasReduction += tmp_long; |
1626 | } |
1880 | } |
- | 1881 | */ // MartinR: deaktiviert Ende |
|
1627 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1882 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1628 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1883 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1629 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1884 | //LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); // MartinR: so war es |
- | 1885 | // MartinR: weshalb unsymmetrisch? |
|
- | 1886 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 32 * STICK_GAIN); // MartinR: geändert |
|
1630 | GasReduction += tmp_int; |
1887 | GasReduction += tmp_int; |
1631 | GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value |
1888 | GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value |
1632 | // ------------------------ ---------------------------------- |
1889 | // ------------------------ ---------------------------------- |
1633 | HCGas -= GasReduction; |
1890 | HCGas -= GasReduction; |
1634 | // limit deviation from hoover point within the target region |
1891 | // limit deviation from hoover point within the target region |
Line 1670... | Line 1927... | ||
1670 | if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) |
1927 | if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) |
1671 | { // old version |
1928 | { // old version |
1672 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1929 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1673 | GasMischanteil = FilterHCGas; |
1930 | GasMischanteil = FilterHCGas; |
1674 | } |
1931 | } |
1675 | else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode |
1932 | //else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode // MartinR: so war es |
- | 1933 | else GasMischanteil = FilterHCGas ; // MartinR: geändert, um Überschwinger bei Höhenänderung zu verringern |
|
1676 | } |
1934 | } |
1677 | }// EOF height control active |
1935 | }// EOF height control active |
1678 | else // HC not active |
1936 | else // HC not active |
1679 | { |
1937 | { |
1680 | //update hoover gas stick value when HC is not active |
1938 | //update hoover gas stick value when HC is not active |
Line 1784... | Line 2042... | ||
1784 | if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2); |
2042 | if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2); |
1785 | } |
2043 | } |
1786 | tmp_int = MAX_GAS*STICK_GAIN; |
2044 | tmp_int = MAX_GAS*STICK_GAIN; |
1787 | if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil)); |
2045 | if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil)); |
1788 | if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil)); |
2046 | if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil)); |
- | 2047 | ||
- | 2048 | ||
- | 2049 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 2050 | // Nick / Roll-Achse // MartinR: um Code zu sparen wurde Nick und Roll zusammengefasst |
|
- | 2051 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 2052 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
|
- | 2053 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
|
Line -... | Line 2054... | ||
- | 2054 | ||
- | 2055 | // PI-Regler für Nick und Roll |
|
- | 2056 | if(EE_Parameter.Gyro_Stability <= 8) |
|
- | 2057 | { |
|
- | 2058 | pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 ; // Zwischenergebnis um Code zu sparen |
|
- | 2059 | pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8; |
|
- | 2060 | } |
|
- | 2061 | else |
|
- | 2062 | { |
|
- | 2063 | pd_ergebnis_nick = ((EE_Parameter.Gyro_Stability / 2) * DiffNick) / 4; // Überlauf verhindern |
|
- | 2064 | pd_ergebnis_roll = ((EE_Parameter.Gyro_Stability / 2) * DiffRoll) / 4; // Überlauf verhindern |
|
- | 2065 | } |
|
- | 2066 | ||
- | 2067 | if(IntegralFaktor) // MartinR : ACC-Mode |
|
- | 2068 | { |
|
- | 2069 | SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
|
- | 2070 | if(SummeNick > (STICK_GAIN * 8000L)) SummeNick = (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf |
|
- | 2071 | if(SummeNick < -(8000L * STICK_GAIN)) SummeNick = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf |
|
- | 2072 | pd_ergebnis_nick += (SummeNick / Ki); // PI-Regler für Nick |
|
- | 2073 | SummeNickHH = 0 ; |
|
- | 2074 | ||
- | 2075 | SummeRoll += IntegralRollMalFaktor - StickRoll; |
|
- | 2076 | if(SummeRoll > (STICK_GAIN * 8000L)) SummeRoll = (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf |
|
- | 2077 | if(SummeRoll < -(8000L * STICK_GAIN)) SummeRoll = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf |
|
- | 2078 | pd_ergebnis_roll += (SummeRoll / Ki); // PI-Regler für Roll |
|
- | 2079 | SummeRollHH = 0; |
|
- | 2080 | ||
- | 2081 | } |
|
- | 2082 | else // MartinR : HH-Mode |
|
- | 2083 | { |
|
- | 2084 | SummeNickHH += DiffNick; // I-Anteil bei HH |
|
- | 2085 | if(SummeNickHH > (STICK_GAIN * 8000L)) SummeNickHH = (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf |
|
- | 2086 | if(SummeNickHH < -(8000L * STICK_GAIN)) SummeNickHH = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf |
|
- | 2087 | pd_ergebnis_nick += SummeNickHH / KiHH; // MartinR: PI-Regler für Nick bei HH |
|
- | 2088 | SummeNick = 0; |
|
- | 2089 | ||
- | 2090 | SummeRollHH += DiffRoll; // I-Anteil bei HH |
|
- | 2091 | if(SummeRollHH > (STICK_GAIN * 8000L)) SummeRollHH = (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf |
|
- | 2092 | if(SummeRollHH < -(8000L * STICK_GAIN)) SummeRollHH = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf |
|
- | 2093 | pd_ergebnis_roll += SummeRollHH / KiHH; // MartinR: PI-Regler für Roll bei HH |
|
- | 2094 | SummeRoll = 0; |
|
- | 2095 | } |
|
- | 2096 | // MartinR : geändert Ende |
|
- | 2097 | ||
- | 2098 | ||
- | 2099 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
|
- | 2100 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
|
- | 2101 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
|
- | 2102 | ||
- | 2103 | //tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
|
- | 2104 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
|
- | 2105 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
|
- | 2106 | ||
- | 2107 | ||
- | 2108 | // MartinR: alt |
|
1789 | 2109 | /* |
|
1790 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2110 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1791 | // Nick-Achse |
2111 | // Nick-Achse |
1792 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2112 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1793 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
2113 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
Line 1819... | Line 2139... | ||
1819 | 2139 | |
|
1820 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
2140 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1821 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
2141 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
Line -... | Line 2142... | ||
- | 2142 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
|
- | 2143 | ||
- | 2144 | */ |
|
1822 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
2145 | // MartinR: alt Ende |
1823 | 2146 | ||
1824 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2147 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1825 | // Universal Mixer |
2148 | // Universal Mixer |
1826 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2149 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1842... | Line 2165... | ||
1842 | // Gier |
2165 | // Gier |
1843 | if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil; |
2166 | if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil; |
1844 | else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil; |
2167 | else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil; |
1845 | else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
2168 | else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
Line 1846... | Line 2169... | ||
1846 | 2169 | ||
- | 2170 | //if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing // MartinR: so war es |
|
1847 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
2171 | if(tmp_int > tmp_motorwert[i]) tmp_int = ((2* tmp_motorwert[i] + tmp_int) / 2) + 1; // MartinR: evtl. stärkere Filterung um Hüpfen bei der Landung zu verringern |
1848 | // else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // original MotorSmoothing |
2172 | // else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // original MotorSmoothing |
1849 | else |
2173 | else |
1850 | { |
2174 | { |
1851 | if(EE_Parameter.MotorSmooth == 0) |
2175 | if(EE_Parameter.MotorSmooth == 0) |