Rev 514 | Rev 604 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 514 | Rev 595 | ||
---|---|---|---|
Line 84... | Line 84... | ||
84 | volatile int DiffNick,DiffRoll; |
84 | volatile int DiffNick,DiffRoll; |
85 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
85 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
86 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
86 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
87 | unsigned char MotorWert[5]; |
87 | unsigned char MotorWert[5]; |
88 | volatile unsigned char SenderOkay = 0; |
88 | volatile unsigned char SenderOkay = 0; |
89 | int StickNick = 0,StickRoll = 0,StickGier = 0; |
89 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
90 | char MotorenEin = 0; |
90 | char MotorenEin = 0; |
91 | int HoehenWert = 0; |
91 | int HoehenWert = 0; |
92 | int SollHoehe = 0; |
92 | int SollHoehe = 0; |
93 | int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
93 | int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
94 | float Ki = FAKTOR_I; |
94 | float Ki = FAKTOR_I; |
Line 420... | Line 420... | ||
420 | 420 | ||
421 | GRN_ON; |
421 | GRN_ON; |
422 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
422 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
423 | // Gaswert ermitteln |
423 | // Gaswert ermitteln |
424 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
425 | GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
424 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
426 | if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20; |
425 | GasMischanteil = StickGas; |
427 | if(GasMischanteil < 0) GasMischanteil = 0; |
426 | if(GasMischanteil < 0) GasMischanteil = 0; |
428 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
427 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
429 | // Emfang schlecht |
428 | // Emfang schlecht |
430 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
429 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 472... | Line 471... | ||
472 | SummeNick = 0; |
471 | SummeNick = 0; |
473 | SummeRoll = 0; |
472 | SummeRoll = 0; |
474 | Mess_Integral_Gier = 0; |
473 | Mess_Integral_Gier = 0; |
475 | Mess_Integral_Gier2 = 0; |
474 | Mess_Integral_Gier2 = 0; |
476 | } |
475 | } |
477 | if((GasMischanteil > 200) && MotorenEin == 0) |
476 | if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
478 | { |
477 | { |
479 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
478 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
480 | // auf Nullwerte kalibrieren |
479 | // auf Nullwerte kalibrieren |
481 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
480 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
482 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
481 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
Line 529... | Line 528... | ||
529 | else delay_neutral = 0; |
528 | else delay_neutral = 0; |
530 | } |
529 | } |
531 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
530 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
532 | // Gas ist unten |
531 | // Gas ist unten |
533 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
532 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
534 | if(GasMischanteil < 35) |
533 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120) |
535 | { |
534 | { |
536 | // Starten |
535 | // Starten |
537 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) |
536 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) |
538 | { |
537 | { |
539 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
538 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 583... | Line 582... | ||
583 | StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P; |
582 | StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P; |
584 | StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
583 | StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
585 | StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P; |
584 | StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P; |
586 | StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
585 | StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
587 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
586 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
- | 587 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
|
- | 588 | ||
588 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
589 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
589 | IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; |
590 | IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; |
Line -... | Line 591... | ||
- | 591 | ||
- | 592 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 593 | //+ Digitale Steuerung per DubWise |
|
590 | 594 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
591 | #define KEY_VALUE (Parameter_UserParam1 * 4) //(Poti3 * 8) |
595 | #define KEY_VALUE (Parameter_UserParam1 * 4) //(Poti3 * 8) |
592 | if(DubWiseKeys[1]) beeptime = 10; |
596 | if(DubWiseKeys[1]) beeptime = 10; |
593 | if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else |
597 | if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else |
594 | if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0; |
598 | if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0; |
Line 603... | Line 607... | ||
603 | if(DubWiseKeys[0] & 16) ExternHoehenValue--; |
607 | if(DubWiseKeys[0] & 16) ExternHoehenValue--; |
Line 604... | Line 608... | ||
604 | 608 | ||
605 | StickNick += ExternStickNick / 8; |
609 | StickNick += ExternStickNick / 8; |
606 | StickRoll += ExternStickRoll / 8; |
610 | StickRoll += ExternStickRoll / 8; |
- | 611 | StickGier += ExternStickGier; |
|
- | 612 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 613 | //+ Analoge Steuerung per Seriell |
|
- | 614 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 615 | if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128) |
|
- | 616 | { |
|
- | 617 | StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
|
- | 618 | StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; |
|
- | 619 | StickGier += ExternControl.Gier; |
|
- | 620 | ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; |
|
- | 621 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
|
Line 607... | Line 622... | ||
607 | StickGier += ExternStickGier; |
622 | } |
608 | 623 | ||
609 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
624 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
610 | if(GyroFaktor < 0) GyroFaktor = 0; |
625 | if(GyroFaktor < 0) GyroFaktor = 0; |
Line 872... | Line 887... | ||
872 | else |
887 | else |
873 | { |
888 | { |
874 | LageKorrekturRoll = 0; |
889 | LageKorrekturRoll = 0; |
875 | LageKorrekturNick = 0; |
890 | LageKorrekturNick = 0; |
876 | } |
891 | } |
- | 892 | ||
- | 893 | // TEST |
|
- | 894 | // LageKorrekturRoll = 0; |
|
- | 895 | // LageKorrekturNick = 0; |
|
- | 896 | // TEST |
|
- | 897 | ||
877 | if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH |
898 | if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH |
878 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
899 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
879 | MittelIntegralNick_Alt = MittelIntegralNick; |
900 | MittelIntegralNick_Alt = MittelIntegralNick; |
880 | MittelIntegralRoll_Alt = MittelIntegralRoll; |
901 | MittelIntegralRoll_Alt = MittelIntegralRoll; |
881 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
902 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1038... | Line 1059... | ||
1038 | } |
1059 | } |
1039 | if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
1060 | if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
1040 | GasMischanteil = hoehenregler; |
1061 | GasMischanteil = hoehenregler; |
1041 | } |
1062 | } |
1042 | } |
1063 | } |
- | 1064 | if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20; |
|
1043 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1065 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1044 | // + Mischer und PI-Regler |
1066 | // + Mischer und PI-Regler |
1045 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1067 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1046 | DebugOut.Analog[7] = GasMischanteil; |
1068 | DebugOut.Analog[7] = GasMischanteil; |
1047 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1069 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |