Rev 742 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 742 | Rev 743 | ||
---|---|---|---|
Line 57... | Line 57... | ||
57 | 57 | ||
58 | unsigned char h,m,s; |
58 | unsigned char h,m,s; |
59 | volatile unsigned int I2CTimeout = 100; |
59 | volatile unsigned int I2CTimeout = 100; |
60 | int MesswertNick,MesswertRoll,MesswertGier; |
60 | int MesswertNick,MesswertRoll,MesswertGier; |
61 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
61 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
Line 62... | Line 62... | ||
62 | int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch; |
62 | int Mittelwert_AccNick, Mittelwert_AccRoll; |
63 | 63 | ||
64 | long IntegralNick = 0,IntegralNick2 = 0; |
64 | long IntegralNick = 0,IntegralNick2 = 0; |
65 | long IntegralRoll = 0,IntegralRoll2 = 0; |
65 | long IntegralRoll = 0,IntegralRoll2 = 0; |
Line 66... | Line 66... | ||
66 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
66 | long IntegralAccNick = 0,IntegralAccRoll = 0; |
67 | long Integral_Gier = 0; |
67 | long Integral_Gier = 0; |
68 | 68 | ||
Line 84... | Line 84... | ||
84 | unsigned char HoehenReglerAktiv = 0; |
84 | unsigned char HoehenReglerAktiv = 0; |
85 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
85 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
Line 86... | Line 86... | ||
86 | 86 | ||
87 | float GyroFaktor; |
87 | float GyroFaktor; |
88 | float IntegralFaktor; |
88 | float IntegralFaktor; |
89 | volatile int DiffNick,DiffRoll; |
89 | int DiffNick,DiffRoll; |
90 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
90 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
91 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
91 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
92 | unsigned char MotorWert[5]; |
92 | unsigned char MotorWert[5]; |
93 | volatile unsigned char SenderOkay = 0; |
93 | volatile unsigned char SenderOkay = 0; |
Line 217... | Line 217... | ||
217 | MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
217 | MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
Line 218... | Line 218... | ||
218 | 218 | ||
219 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
219 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
220 | Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
220 | Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
221 | Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L; |
- | |
222 | Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L; |
221 | Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L; |
223 | IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; |
222 | IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; |
224 | IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
- | |
225 | IntegralAccZ += Aktuell_az - acc_neutral.Z; |
223 | IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
226 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
224 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
227 | Mess_Integral_Gier += MesswertGier; |
225 | Mess_Integral_Gier += MesswertGier; |
228 | Mess_Integral_Gier2 += MesswertGier; |
226 | Mess_Integral_Gier2 += MesswertGier; |
229 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
227 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
Line 331... | Line 329... | ||
331 | MesswertNick = AdWertNick; |
329 | MesswertNick = AdWertNick; |
332 | MesswertRoll = AdWertRoll; |
330 | MesswertRoll = AdWertRoll; |
333 | MesswertGier = AdWertGier; |
331 | MesswertGier = AdWertGier; |
334 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
332 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
335 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
333 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
336 | Mittelwert_AccHoch = (long)AdWertAccHoch; |
- | |
337 | // ADC einschalten |
334 | // ADC einschalten |
338 | ANALOG_ON; |
335 | ANALOG_ON; |
339 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
336 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
340 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
337 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
341 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
338 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
Line 344... | Line 341... | ||
344 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
341 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
345 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
342 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
346 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
343 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
Line 347... | Line 344... | ||
347 | 344 | ||
348 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
345 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
349 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
346 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
Line 350... | Line 347... | ||
350 | } |
347 | } |
351 | 348 | ||
352 | //############################################################################ |
349 | //############################################################################ |
Line 456... | Line 453... | ||
456 | MotorenEin = 0; |
453 | MotorenEin = 0; |
457 | Notlandung = 0; |
454 | Notlandung = 0; |
458 | } |
455 | } |
459 | ROT_ON; |
456 | ROT_ON; |
460 | if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
457 | if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
461 | { |
458 | { |
462 | GasMischanteil = EE_Parameter.NotGas; |
459 | GasMischanteil = EE_Parameter.NotGas; |
463 | Notlandung = 1; |
460 | Notlandung = 1; |
- | 461 | PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
|
- | 462 | PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
|
464 | PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
463 | PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
465 | PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
464 | PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
466 | PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; |
465 | PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; |
467 | } |
466 | } |
468 | else MotorenEin = 0; |
467 | else MotorenEin = 0; |
469 | } |
468 | } |
470 | else |
469 | else |
471 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
470 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
472 | // Emfang gut |
471 | // Emfang gut |
473 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
472 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 506... | Line 505... | ||
506 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2; |
505 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2; |
507 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; |
506 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; |
508 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; |
507 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; |
509 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
508 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
510 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
509 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
511 | } |
510 | } |
512 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
- | |
513 | { |
- | |
514 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
- | |
515 | } |
- | |
516 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], sizeof(struct mk_param_struct)); |
511 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], sizeof(struct mk_param_struct)); |
517 | SetNeutral(); |
512 | SetNeutral(); |
518 | Piep(GetActiveParamSetNumber()); |
513 | Piep(GetActiveParamSetNumber()); |
519 | } |
514 | } |
520 | } |
515 | } |
Line 751... | Line 746... | ||
751 | { |
746 | { |
752 | MittelIntegralNick /= ABGLEICH_ANZAHL; |
747 | MittelIntegralNick /= ABGLEICH_ANZAHL; |
753 | MittelIntegralRoll /= ABGLEICH_ANZAHL; |
748 | MittelIntegralRoll /= ABGLEICH_ANZAHL; |
754 | IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
749 | IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
755 | IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
750 | IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
756 | IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL; |
- | |
757 | #define MAX_I 0//(Poti2/10) |
751 | #define MAX_I 0//(Poti2/10) |
758 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
752 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
759 | IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick); |
753 | IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick); |
760 | ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; |
754 | ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; |
761 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
755 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 766... | Line 760... | ||
766 | LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
760 | LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
Line 767... | Line 761... | ||
767 | 761 | ||
768 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
762 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
769 | { |
763 | { |
770 | LageKorrekturNick /= 2; |
764 | LageKorrekturNick /= 2; |
771 | LageKorrekturNick /= 2; |
765 | LageKorrekturRoll /= 2; |
Line 772... | Line 766... | ||
772 | } |
766 | } |
773 | 767 | ||
774 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
768 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 869... | Line 863... | ||
869 | MittelIntegralNick_Alt = MittelIntegralNick; |
863 | MittelIntegralNick_Alt = MittelIntegralNick; |
870 | MittelIntegralRoll_Alt = MittelIntegralRoll; |
864 | MittelIntegralRoll_Alt = MittelIntegralRoll; |
871 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
865 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
872 | IntegralAccNick = 0; |
866 | IntegralAccNick = 0; |
873 | IntegralAccRoll = 0; |
867 | IntegralAccRoll = 0; |
874 | IntegralAccZ = 0; |
- | |
875 | MittelIntegralNick = 0; |
868 | MittelIntegralNick = 0; |
876 | MittelIntegralRoll = 0; |
869 | MittelIntegralRoll = 0; |
877 | MittelIntegralNick2 = 0; |
870 | MittelIntegralNick2 = 0; |
878 | MittelIntegralRoll2 = 0; |
871 | MittelIntegralRoll2 = 0; |
879 | ZaehlMessungen = 0; |
872 | ZaehlMessungen = 0; |
Line 952... | Line 945... | ||
952 | 945 | ||
953 | DebugOut.Analog[12] = Motor_Vorne; |
946 | DebugOut.Analog[12] = Motor_Vorne; |
954 | DebugOut.Analog[13] = Motor_Hinten; |
947 | DebugOut.Analog[13] = Motor_Hinten; |
955 | DebugOut.Analog[14] = Motor_Links; |
948 | DebugOut.Analog[14] = Motor_Links; |
956 | DebugOut.Analog[15] = Motor_Rechts; |
949 | DebugOut.Analog[15] = Motor_Rechts; |
Line 957... | Line 950... | ||
957 | DebugOut.Analog[16] = Mittelwert_AccHoch; |
950 | DebugOut.Analog[16] = AdWertAccHoch; |
958 | 951 | ||
959 | DebugOut.Analog[17] = GPSInfo.satfix; |
952 | DebugOut.Analog[17] = GPSInfo.satfix; |
960 | DebugOut.Analog[18] = GPSInfo.utmnorth; |
953 | DebugOut.Analog[18] = GPSInfo.utmnorth; |