Rev 1667 | Rev 1669 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1667 | Rev 1668 | ||
|---|---|---|---|
| Line 64... | Line 64... | ||
| 64 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
64 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
| 65 | int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch; |
65 | int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch; |
| 66 | unsigned int NeutralAccX=0, NeutralAccY=0; |
66 | unsigned int NeutralAccX=0, NeutralAccY=0; |
| 67 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
67 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
| 68 | int NeutralAccZ = 0; |
68 | int NeutralAccZ = 0; |
| 69 | unsigned char ControlHeading = 0; |
69 | unsigned char ControlHeading = 0;// in 0,2° |
| 70 | long IntegralNick = 0,IntegralNick2 = 0; |
70 | long IntegralNick = 0,IntegralNick2 = 0; |
| 71 | long IntegralRoll = 0,IntegralRoll2 = 0; |
71 | long IntegralRoll = 0,IntegralRoll2 = 0; |
| 72 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
72 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
| 73 | long Integral_Gier = 0; |
73 | long Integral_Gier = 0; |
| 74 | long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
74 | long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
| Line 144... | Line 144... | ||
| 144 | unsigned char Parameter_NaviGpsACC; |
144 | unsigned char Parameter_NaviGpsACC; |
| 145 | unsigned char Parameter_NaviOperatingRadius; |
145 | unsigned char Parameter_NaviOperatingRadius; |
| 146 | unsigned char Parameter_NaviWindCorrection; |
146 | unsigned char Parameter_NaviWindCorrection; |
| 147 | unsigned char Parameter_NaviSpeedCompensation; |
147 | unsigned char Parameter_NaviSpeedCompensation; |
| 148 | unsigned char Parameter_ExternalControl; |
148 | unsigned char Parameter_ExternalControl; |
| - | 149 | unsigned char Parameter_OrientationModeControl; |
|
| 149 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
150 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
| 150 | unsigned char HeadFree = 0; |
151 | unsigned char CareFree = 0; |
| Line 151... | Line 152... | ||
| 151 | 152 | ||
| 152 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
153 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
| 153 | int MaxStickNick = 0,MaxStickRoll = 0; |
154 | int MaxStickNick = 0,MaxStickRoll = 0; |
| 154 | unsigned int modell_fliegt = 0; |
155 | unsigned int modell_fliegt = 0; |
| Line 550... | Line 551... | ||
| 550 | //############################################################################ |
551 | //############################################################################ |
| 551 | // Trägt ggf. das Poti als Parameter ein |
552 | // Trägt ggf. das Poti als Parameter ein |
| 552 | void ParameterZuordnung(void) |
553 | void ParameterZuordnung(void) |
| 553 | //############################################################################ |
554 | //############################################################################ |
| 554 | { |
555 | { |
| 555 | - | ||
| - | 556 | unsigned char tmp; |
|
| 556 | #define CHK_POTI(b,a) {if(a < 248) b = a; else b = Poti[255 - a];} |
557 | #define CHK_POTI(b,a) {if(a < 248) b = a; else b = Poti[255 - a];} |
| 557 | #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max);} |
558 | #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max);} |
| Line 558... | Line 559... | ||
| 558 | 559 | ||
| 559 | CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
560 | CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
| Line 583... | Line 584... | ||
| 583 | CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7); |
584 | CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7); |
| 584 | CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8); |
585 | CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8); |
| 585 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl); |
586 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl); |
| 586 | CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl); |
587 | CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl); |
| 587 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit); |
588 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit); |
| 588 | CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1); |
589 | CHK_POTI(Parameter_AchsKopplung1,EE_Parameter.AchsKopplung1); |
| 589 | CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2); |
590 | CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2); |
| 590 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
591 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
| 591 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
592 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
| 592 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
593 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
| 593 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
594 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
| 594 | Ki = (10300 / 2) / (Parameter_I_Faktor + 1); |
595 | Ki = (10300 / 2) / (Parameter_I_Faktor + 1); |
| 595 | MAX_GAS = EE_Parameter.Gas_Max; |
596 | MAX_GAS = EE_Parameter.Gas_Max; |
| 596 | MIN_GAS = EE_Parameter.Gas_Min; |
597 | MIN_GAS = EE_Parameter.Gas_Min; |
| - | 598 | ||
| - | 599 | tmp = EE_Parameter.OrientationModeControl; |
|
| - | 600 | if(tmp > 50 && NaviDataOkay > 200) |
|
| - | 601 | { |
|
| - | 602 | CareFree = 1; |
|
| 597 | if(Poti4 > 50) HeadFree = 1; else HeadFree = 0; |
603 | if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0; |
| - | 604 | } |
|
| - | 605 | else CareFree = 0; |
|
| - | 606 | ||
| 598 | if(HeadFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;} |
607 | if(CareFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;} |
| 599 | } |
608 | } |
| Line 600... | Line 609... | ||
| 600 | 609 | ||
| 601 | //############################################################################ |
610 | //############################################################################ |
| 602 | // |
611 | // |
| Line 765... | Line 774... | ||
| 765 | Mess_IntegralNick2 = IntegralNick; |
774 | Mess_IntegralNick2 = IntegralNick; |
| 766 | Mess_IntegralRoll2 = IntegralRoll; |
775 | Mess_IntegralRoll2 = IntegralRoll; |
| 767 | SummeNick = 0; |
776 | SummeNick = 0; |
| 768 | SummeRoll = 0; |
777 | SummeRoll = 0; |
| 769 | FCFlags |= FCFLAG_START; |
778 | FCFlags |= FCFLAG_START; |
| 770 | ControlHeading = KompassValue; |
779 | ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
| 771 | } |
780 | } |
| 772 | else |
781 | else |
| 773 | { |
782 | { |
| 774 | beeptime = 1500; // indicate missing calibration |
783 | beeptime = 1500; // indicate missing calibration |
| 775 | } |
784 | } |
| Line 800... | Line 809... | ||
| 800 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
809 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 801... | Line 810... | ||
| 801 | 810 | ||
| 802 | if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG)) |
811 | if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG)) |
| 803 | { |
812 | { |
| 804 | static int stick_nick,stick_roll; |
- | |
| 805 | /* |
- | |
| 806 | unsigned char angle = 180/15; |
- | |
| 807 | signed char sintab[31] = { 0, 4, 8, 11, 14, 16, 16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16, 16}; |
- | |
| 808 | //signed char costab[24] = {16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16}; |
- | |
| 809 | angle = (ControlHeading + (360/15) - ErsatzKompass / (GIER_GRAD_FAKTOR * 15)) % 24; |
- | |
| 810 | - | ||
| 811 | signed int cos_h, sin_h; |
- | |
| 812 | cos_h = sintab[angle + 6]/2; |
- | |
| 813 | sin_h = sintab[angle]/2; |
- | |
| 814 | */ |
813 | static int stick_nick,stick_roll; |
| 815 | ParameterZuordnung(); |
814 | ParameterZuordnung(); |
| 816 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
815 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
| 817 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
816 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
| 818 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
817 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
| Line 819... | Line -... | ||
| 819 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
- | |
| Line -... | Line 818... | ||
| - | 818 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
|
| - | 819 | ||
| 820 | 820 | ||
| 821 | DebugOut.Analog[16] = stick_roll; |
821 | 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}; |
| 822 | 822 | ||
| 823 | if(HeadFree) |
823 | if(CareFree) |
| 824 | { |
824 | { |
| 825 | signed int nick, roll; |
825 | signed int nick, roll; |
| 826 | nick = stick_nick / 4; |
826 | nick = stick_nick / 4; |
| 827 | roll = stick_roll / 4; |
827 | roll = stick_roll / 4; |
| 828 | StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4); |
828 | StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4); |
| 829 | StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4); |
829 | StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4); |
| - | 830 | } |
|
| - | 831 | else |
|
| 830 | } |
832 | { |
| 831 | else |
833 | FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6]; |
| 832 | { |
834 | FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; |
| 833 | StickNick = stick_nick; |
- | |
| - | 835 | StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8; |
|
| Line 834... | Line 836... | ||
| 834 | StickRoll = stick_roll; |
836 | StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8; |
| 835 | } |
837 | } |
| 836 | DebugOut.Analog[17] = StickRoll; |
838 | |