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 |