Rev 393 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 393 | Rev 414 | ||
---|---|---|---|
Line 202... | Line 202... | ||
202 | IntegralNick2 = Mess_IntegralNick2; |
202 | IntegralNick2 = Mess_IntegralNick2; |
203 | IntegralRoll2 = Mess_IntegralRoll2; |
203 | IntegralRoll2 = Mess_IntegralRoll2; |
204 | // ADC einschalten |
204 | // ADC einschalten |
205 | ANALOG_ON; |
205 | ANALOG_ON; |
Line 206... | Line -... | ||
206 | - | ||
207 | /* |
206 | |
- | 207 | //------------------------------------------------------------------------------ |
|
- | 208 | if(!Looping_Nick && !Looping_Roll) //übernommen aus der V0.66b //(071107Kr) |
|
208 | //------------------------------------------------------------------------------ |
209 | { |
209 | if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
210 | if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
210 | else |
211 | else |
- | 212 | if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
|
211 | if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
213 | |
212 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
214 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
213 | else |
215 | else |
- | 216 | if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
|
214 | if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
217 | } |
215 | //------------------------------------------------------------------------------ |
218 | //------------------------------------------------------------------------------ |
216 | */ |
219 | |
217 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
220 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
218 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
221 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
219 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
222 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
220 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
223 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
Line 454... | Line 457... | ||
454 | { |
457 | { |
455 | int motorwert,pd_ergebnis,h,tmp_int; |
458 | int motorwert,pd_ergebnis,h,tmp_int; |
456 | int GierMischanteil,GasMischanteil; |
459 | int GierMischanteil,GasMischanteil; |
457 | static long SummeNick=0,SummeRoll=0; |
460 | static long SummeNick=0,SummeRoll=0; |
458 | static long sollGier = 0,tmp_long,tmp_long2; |
461 | static long sollGier = 0,tmp_long,tmp_long2; |
459 | long IntegralFehlerNick = 0; |
462 | static long IntegralFehlerNick = 0; |
460 | long IntegralFehlerRoll = 0; |
463 | static long IntegralFehlerRoll = 0; |
461 | static unsigned int RcLostTimer; |
464 | static unsigned int RcLostTimer; |
462 | static unsigned char delay_neutral = 0; |
465 | static unsigned char delay_neutral = 0; |
463 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
466 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
464 | static unsigned int modell_fliegt = 0; |
467 | static unsigned int modell_fliegt = 0; |
465 | static int hoehenregler = 0; |
468 | static int hoehenregler = 0; |
Line 619... | Line 622... | ||
619 | 622 | ||
620 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
623 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
621 | if(GyroFaktor < 0) GyroFaktor = 0; |
624 | if(GyroFaktor < 0) GyroFaktor = 0; |
622 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
625 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
- | 626 | // greift in den Stick ein, um ungewolltes überschlagen zu verhindern |
|
623 | // greift in den Stick ein, um ungewolltes überschlagen zu verhindern |
627 | //bei aktiviertem GPS dürfen die Stickwerte nicht beeinflusst werden, da ansonsten ein Neulernen der Sollposition stattfinden könnte //(051107Kr) |
624 | if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS)) |
628 | if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) && !(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) //(051107Kr) |
625 | { |
629 | { |
626 | if(IntegralNick > 60000) |
630 | if(IntegralNick > 60000) |
627 | { |
631 | { |
628 | StickNick -= 8 * EE_Parameter.Stick_P; |
632 | StickNick -= 8 * EE_Parameter.Stick_P; |
Line 649... | Line 653... | ||
649 | 653 | ||
650 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
654 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
651 | // LED Stuff |
655 | // LED Stuff |
Line -... | Line 656... | ||
- | 656 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 657 | ||
652 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
658 | #define LED1_ONTIME 20 |
653 | 659 | ||
654 | LED1_OFF; |
660 | LED1_OFF; |
655 | 661 | ||
656 | if(blinkcount_LED1 >= Parameter_UserParam4 && Parameter_UserParam4 > 0) |
662 | if(Parameter_UserParam4 >= 200) |
657 | { |
663 | { |
658 | LED1_FLASH; |
664 | LED1_ON; |
- | 665 | blinkcount_LED1 = 0; |
|
- | 666 | } |
|
- | 667 | ||
- | 668 | else if(blinkcount_LED1 >= Parameter_UserParam4 && Parameter_UserParam4 > 0 && MotorenEin == 1) |
|
- | 669 | { |
|
- | 670 | LED1_ON; |
|
- | 671 | ||
- | 672 | if(blinkcount_LED1 >= LED1_ONTIME + Parameter_UserParam4) |
|
- | 673 | { |
|
- | 674 | LED1_OFF; |
|
- | 675 | blinkcount_LED1 = 0; |
|
Line 659... | Line 676... | ||
659 | blinkcount_LED1 = 0; |
676 | } |
Line 660... | Line 677... | ||
660 | } |
677 | } |
661 | 678 | ||
Line 966... | Line 983... | ||
966 | DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick)); // Differenz bestimmen |
983 | DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick)); // Differenz bestimmen |
967 | SummeNick += DiffNick; // I-Anteil |
984 | SummeNick += DiffNick; // I-Anteil |
968 | if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1; |
985 | if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1; |
969 | if(SummeNick > 16000) SummeNick = 16000; |
986 | if(SummeNick > 16000) SummeNick = 16000; |
970 | if(SummeNick < -16000) SummeNick = -16000; |
987 | if(SummeNick < -16000) SummeNick = -16000; |
971 | pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick |
988 | pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
972 | // Motor Vorn |
989 | // Motor Vorn |
973 | #define MUL 2 |
990 | #define MUL 2 |
974 | if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
991 | if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
975 | if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil)); |
992 | if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil)); |
Line 991... | Line 1008... | ||
991 | DiffRoll = Kp * (MesswertRoll - (StickRoll - GPS_Roll)); // Differenz bestimmen |
1008 | DiffRoll = Kp * (MesswertRoll - (StickRoll - GPS_Roll)); // Differenz bestimmen |
992 | SummeRoll += DiffRoll; // I-Anteil |
1009 | SummeRoll += DiffRoll; // I-Anteil |
993 | if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1; |
1010 | if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1; |
994 | if(SummeRoll > 16000) SummeRoll = 16000; |
1011 | if(SummeRoll > 16000) SummeRoll = 16000; |
995 | if(SummeRoll < -16000) SummeRoll = -16000; |
1012 | if(SummeRoll < -16000) SummeRoll = -16000; |
996 | pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll |
1013 | pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
997 | if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
1014 | if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
998 | if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil)); |
1015 | if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil)); |
999 | // Motor Links |
1016 | // Motor Links |
1000 | motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
1017 | motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
1001 | if ((motorwert < 0)) motorwert = 0; |
1018 | if ((motorwert < 0)) motorwert = 0; |