Rev 1210 | Rev 1212 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1210 | Rev 1211 | ||
---|---|---|---|
Line 84... | Line 84... | ||
84 | unsigned char TrichterFlug = 0; |
84 | unsigned char TrichterFlug = 0; |
85 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
85 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
86 | long ErsatzKompass; |
86 | long ErsatzKompass; |
87 | int ErsatzKompassInGrad; // Kompasswert in Grad |
87 | int ErsatzKompassInGrad; // Kompasswert in Grad |
88 | int GierGyroFehler = 0; |
88 | int GierGyroFehler = 0; |
89 | char GyroFaktor; |
89 | char GyroFaktor,GyroFaktorGier; |
90 | char IntegralFaktor; |
90 | char IntegralFaktor,IntegralFaktorGier; |
91 | int DiffNick,DiffRoll; |
91 | int DiffNick,DiffRoll; |
92 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
92 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
93 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links; |
93 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links; |
94 | volatile unsigned char Motor1, Motor2,Motor3,Motor4,Motor5,Motor6,Motor7,Motor8; |
94 | volatile unsigned char Motor1, Motor2,Motor3,Motor4,Motor5,Motor6,Motor7,Motor8; |
95 | volatile unsigned char SenderOkay = 0; |
95 | volatile unsigned char SenderOkay = 0; |
Line 148... | Line 148... | ||
148 | unsigned int modell_fliegt = 0; |
148 | unsigned int modell_fliegt = 0; |
149 | volatile unsigned char MikroKopterFlags = 0; |
149 | volatile unsigned char MikroKopterFlags = 0; |
150 | long GIER_GRAD_FAKTOR = 1291; |
150 | long GIER_GRAD_FAKTOR = 1291; |
151 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
151 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
152 | unsigned char RequiredMotors = 4; |
152 | unsigned char RequiredMotors = 4; |
153 | - | ||
154 | /* |
- | |
155 | signed char Mixer.Motor[MAX_MOTORS][4] = { |
- | |
156 | {64, +64, 0, +64},//1 |
- | |
157 | {64, +64, -64, -64},//2 |
- | |
158 | {64, 0, -64, +64},//3 |
- | |
159 | {64, -64, -64, -64},//4 |
- | |
160 | {64, -64, 0, +64},//5 |
- | |
161 | {64, -64, +64, -64},//6 |
- | |
162 | {64, 0, +64, +64},//7 |
- | |
163 | {64, +64, +64, -64},//8 |
- | |
164 | { 0, 0, 0, 0},//9 |
- | |
165 | { 0, 0, 0, 0},//10 |
- | |
166 | { 0, 0, 0, 0},//11 |
- | |
167 | { 0, 0, 0, 0}};//12 |
- | |
168 | */ |
- | |
169 | /* |
- | |
170 | signed char Mixer.Motor[MAX_MOTORS][4] = { |
- | |
171 | { 64, +64, 0, +64},//1 |
- | |
172 | { 64, -64, 0, +64},//2 |
- | |
173 | { 64, 0, -64, -64},//3 |
- | |
174 | { 64, 0, +64, -64},//4 |
- | |
175 | { 0, 0, 0, 0},//5 |
- | |
176 | { 0, 0, 0, 0},//6 |
- | |
177 | { 0, 0, 0, 0},//7 |
- | |
178 | { 0, 0, 0, 0},//8 |
- | |
179 | { 0, 0, 0, 0},//9 |
- | |
180 | { 0, 0, 0, 0},//10 |
- | |
181 | { 0, 0, 0, 0},//11 |
- | |
182 | { 0, 0, 0, 0}};//12 |
- | |
183 | */ |
- | |
184 | unsigned char Motor[MAX_MOTORS]; |
153 | unsigned char Motor[MAX_MOTORS]; |
185 | signed int tmp_motorwert[MAX_MOTORS]; |
154 | signed int tmp_motorwert[MAX_MOTORS]; |
Line 186... | Line 155... | ||
186 | 155 | ||
187 | int MotorSmoothing(int neu, int alt) |
156 | int MotorSmoothing(int neu, int alt) |
Line 583... | Line 552... | ||
583 | static long IntegralFehlerRoll = 0; |
552 | static long IntegralFehlerRoll = 0; |
584 | static unsigned int RcLostTimer; |
553 | static unsigned int RcLostTimer; |
585 | static unsigned char delay_neutral = 0; |
554 | static unsigned char delay_neutral = 0; |
586 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
555 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
587 | static int hoehenregler = 0; |
556 | static int hoehenregler = 0; |
588 | // static int motorwert1,motorwert2,motorwert3,motorwert4,motorwert5,motorwert6,motorwert7,motorwert8; |
- | |
589 | static char TimerWerteausgabe = 0; |
557 | static char TimerWerteausgabe = 0; |
590 | static char NeueKompassRichtungMerken = 0; |
558 | static char NeueKompassRichtungMerken = 0; |
591 | static long ausgleichNick, ausgleichRoll; |
559 | static long ausgleichNick, ausgleichRoll; |
592 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
560 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
593 | unsigned char i; |
561 | unsigned char i; |
Line 769... | Line 737... | ||
769 | // neue Werte von der Funke |
737 | // neue Werte von der Funke |
770 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
738 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 771... | Line 739... | ||
771 | 739 | ||
772 | if(!NewPpmData-- || Notlandung) |
740 | if(!NewPpmData-- || Notlandung) |
773 | { |
- | |
774 | int tmp_int; |
741 | { |
775 | static int stick_nick,stick_roll; |
742 | static int stick_nick,stick_roll; |
776 | ParameterZuordnung(); |
743 | ParameterZuordnung(); |
777 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
744 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
778 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
745 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
Line 785... | Line 752... | ||
785 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
752 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
786 | if(StickGier > 2) StickGier -= 2; else |
753 | if(StickGier > 2) StickGier -= 2; else |
787 | if(StickGier < -2) StickGier += 2; else StickGier = 0; |
754 | if(StickGier < -2) StickGier += 2; else StickGier = 0; |
Line 788... | Line 755... | ||
788 | 755 | ||
789 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
- | |
790 | - | ||
791 | /* if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick) |
- | |
792 | MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--; |
- | |
793 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll) |
- | |
794 | MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--; |
- | |
795 | */ |
- | |
796 | // GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / (256.0/STICK_GAIN); |
756 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
797 | GyroFaktor = (Parameter_Gyro_P + 10.0); |
- | |
798 | // IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN); |
757 | GyroFaktor = (Parameter_Gyro_P + 10.0); |
- | 758 | IntegralFaktor = Parameter_Gyro_I; |
|
- | 759 | GyroFaktorGier = (Parameter_Gyro_P + 10.0); |
|
Line 799... | Line 760... | ||
799 | IntegralFaktor = Parameter_Gyro_I; |
760 | IntegralFaktorGier = Parameter_Gyro_I; |
800 | 761 | ||
801 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
762 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
802 | //+ Analoge Steuerung per Seriell |
763 | //+ Analoge Steuerung per Seriell |
Line 882... | Line 843... | ||
882 | if(Notlandung) |
843 | if(Notlandung) |
883 | { |
844 | { |
884 | StickGier = 0; |
845 | StickGier = 0; |
885 | StickNick = 0; |
846 | StickNick = 0; |
886 | StickRoll = 0; |
847 | StickRoll = 0; |
887 | GyroFaktor = 90;//(float) 100 / (256.0 / STICK_GAIN); |
848 | GyroFaktor = 90; |
888 | IntegralFaktor = 120;//(float) 120 / (44000 / STICK_GAIN); |
849 | IntegralFaktor = 120; |
- | 850 | GyroFaktorGier = 90; |
|
- | 851 | IntegralFaktorGier = 120; |
|
889 | Looping_Roll = 0; |
852 | Looping_Roll = 0; |
890 | Looping_Nick = 0; |
853 | Looping_Nick = 0; |
891 | } |
854 | } |
Line 1173... | Line 1136... | ||
1173 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1136 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1174 | if(abs(MesswertGier) > 128) |
1137 | if(abs(MesswertGier) > 128) |
1175 | { |
1138 | { |
1176 | fehler = 0; |
1139 | fehler = 0; |
1177 | } |
1140 | } |
1178 | - | ||
1179 | if(NeueKompassRichtungMerken) |
- | |
1180 | { |
- | |
1181 | // ErsatzKompass += (fehler * 32) / korrektur; |
- | |
1182 | // fehler = 0; |
- | |
1183 | // fehler /= 4; |
- | |
1184 | // ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
- | |
1185 | } |
- | |
1186 | if(!KompassSignalSchlecht && w < 25) |
1141 | if(!KompassSignalSchlecht && w < 25) |
1187 | { |
1142 | { |
1188 | GierGyroFehler += fehler; |
1143 | GierGyroFehler += fehler; |
1189 | if(NeueKompassRichtungMerken) |
1144 | if(NeueKompassRichtungMerken) |
1190 | { |
1145 | { |
Line 1287... | Line 1242... | ||
1287 | 1242 | ||
1288 | #define TRIM_MAX 200 |
1243 | #define TRIM_MAX 200 |
1289 | if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
1244 | if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
Line 1290... | Line -... | ||
1290 | if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
- | |
1291 | 1245 | if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
|
1292 | { |
1246 | |
1293 | MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
- | |
1294 | MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
- | |
1295 | } |
- | |
1296 | - | ||
1297 | #ifndef QUADRO |
- | |
1298 | MesswertGier = (long)(MesswertGier * 4 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (4 * (44000 / STICK_GAIN)); |
1247 | MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
1299 | #else |
- | |
Line 1300... | Line 1248... | ||
1300 | MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (2 * (44000 / STICK_GAIN)); |
1248 | MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
1301 | #endif |
1249 | MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
1302 | 1250 | ||
1303 | // Maximalwerte abfangen |
1251 | // Maximalwerte abfangen |
Line 1374... | Line 1322... | ||
1374 | h = HoehenWert; |
1322 | h = HoehenWert; |
1375 | if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
1323 | if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
1376 | { |
1324 | { |
1377 | h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil |
1325 | h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil |
1378 | h = GasMischanteil - h; // vom Gas abziehen |
1326 | h = GasMischanteil - h; // vom Gas abziehen |
1379 | // h -= (HoeheD * Parameter_Luftdruck_D)/(8/STICK_GAIN); // D-Anteil |
- | |
1380 | h -= (HoeheD)/(8/STICK_GAIN); // D-Anteil |
1327 | h -= (HoeheD)/(8/STICK_GAIN); // D-Anteil |
1381 | tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1328 | tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1382 | if(tmp_int > 70*STICK_GAIN) tmp_int = 70*STICK_GAIN; |
1329 | if(tmp_int > 70*STICK_GAIN) tmp_int = 70*STICK_GAIN; |
1383 | else if(tmp_int < -(70*STICK_GAIN)) tmp_int = -(70*STICK_GAIN); |
1330 | else if(tmp_int < -(70*STICK_GAIN)) tmp_int = -(70*STICK_GAIN); |
1384 | h -= tmp_int; |
1331 | h -= tmp_int; |