Rev 805 | Rev 820 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 805 | Rev 819 | ||
---|---|---|---|
Line 85... | Line 85... | ||
85 | float GyroFaktor; |
85 | float GyroFaktor; |
86 | float IntegralFaktor; |
86 | float IntegralFaktor; |
87 | volatile int DiffNick,DiffRoll; |
87 | volatile int DiffNick,DiffRoll; |
88 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
88 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
89 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
89 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
90 | unsigned char MotorWert[5]; |
- | |
91 | volatile unsigned char SenderOkay = 0; |
90 | volatile unsigned char SenderOkay = 0; |
92 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
91 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
93 | char MotorenEin = 0; |
92 | char MotorenEin = 0; |
94 | int HoehenWert = 0; |
93 | int HoehenWert = 0; |
95 | int SollHoehe = 0; |
94 | int SollHoehe = 0; |
Line 410... | Line 409... | ||
410 | MAX_GAS = EE_Parameter.Gas_Max; |
409 | MAX_GAS = EE_Parameter.Gas_Max; |
411 | MIN_GAS = EE_Parameter.Gas_Min; |
410 | MIN_GAS = EE_Parameter.Gas_Min; |
412 | } |
411 | } |
Line -... | Line 412... | ||
- | 412 | ||
- | 413 | ||
413 | 414 | ||
414 | 415 | ||
415 | //############################################################################ |
416 | //############################################################################ |
416 | // |
417 | // |
417 | void MotorRegler(void) |
418 | void MotorRegler(void) |
Line 513... | Line 514... | ||
513 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; |
514 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; |
514 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; |
515 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; |
515 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
516 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
516 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
517 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
517 | } |
518 | } |
- | 519 | else |
|
518 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
520 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 20 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) |
- | 521 | { |
|
- | 522 | WinkelOut.CalcState = 1; |
|
- | 523 | beeptime = 1000; |
|
- | 524 | } |
|
- | 525 | else |
|
519 | { |
526 | { |
- | 527 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
|
- | 528 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
|
- | 529 | { |
|
520 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
530 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
521 | } |
531 | } |
522 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
- | |
523 | SetNeutral(); |
532 | SetNeutral(); |
524 | Piep(GetActiveParamSetNumber()); |
533 | Piep(GetActiveParamSetNumber()); |
- | 534 | } |
|
525 | } |
535 | } |
526 | } |
536 | } |
527 | else |
537 | else |
528 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
538 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
529 | { |
539 | { |
Line 821... | Line 831... | ||
821 | Mess_IntegralNick2 -= IntegralFehlerNick; |
831 | Mess_IntegralNick2 -= IntegralFehlerNick; |
822 | Mess_IntegralRoll2 -= IntegralFehlerRoll; |
832 | Mess_IntegralRoll2 -= IntegralFehlerRoll; |
Line 823... | Line 833... | ||
823 | 833 | ||
824 | // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
834 | // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
825 | // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
- | |
826 | DebugOut.Analog[10] = GierGyroFehler; |
835 | // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
827 | if(GierGyroFehler > ABGLEICH_ANZAHL/2) AdNeutralGier++; |
836 | if(GierGyroFehler > ABGLEICH_ANZAHL/2) AdNeutralGier++; |
Line 828... | Line 837... | ||
828 | if(GierGyroFehler <-ABGLEICH_ANZAHL/2) AdNeutralGier--; |
837 | if(GierGyroFehler <-ABGLEICH_ANZAHL/2) AdNeutralGier--; |
Line 946... | Line 955... | ||
946 | // Gieren |
955 | // Gieren |
947 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
956 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
948 | // if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
957 | // if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
949 | if(abs(StickGier) > 20) // war 35 |
958 | if(abs(StickGier) > 20) // war 35 |
950 | { |
959 | { |
951 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) { NeueKompassRichtungMerken = 1; KompassSignalSchlecht = 500;}; |
960 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) { NeueKompassRichtungMerken = 1; KompassSignalSchlecht = 250;}; |
952 | } |
961 | } |
953 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
962 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
954 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
963 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
955 | sollGier = tmp_int; |
964 | sollGier = tmp_int; |
956 | Mess_Integral_Gier -= tmp_int; |
965 | Mess_Integral_Gier -= tmp_int; |
Line 958... | Line 967... | ||
958 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
967 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
Line 959... | Line 968... | ||
959 | 968 | ||
960 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
969 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
961 | // Kompass |
970 | // Kompass |
- | 971 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 972 | DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
|
962 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
973 | |
963 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
974 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
964 | { |
975 | { |
965 | int w,v,fehler,korrektur; |
976 | int w,v,r,fehler,korrektur; |
966 | static int KompassSignalSchlecht = 0; |
977 | static int KompassSignalSchlecht = 0; |
967 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
978 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
968 | v = abs(IntegralRoll /512); |
979 | v = abs(IntegralRoll /512); |
969 | //v /= 4; |
980 | //v /= 4; |
970 | //w /= 4; |
981 | //w /= 4; |
- | 982 | if(v > w) w = v; // grösste Neigung ermitteln |
|
971 | if(v > w) w = v; // grösste Neigung ermitteln |
983 | // if((MaxStickNick > 32) || (MaxStickRoll > 32)) w *= 2; |
972 | korrektur = w + 4; |
984 | korrektur = w + 4; |
973 | if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht) |
985 | if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht) |
974 | { |
986 | { |
975 | KompassStartwert = KompassValue; |
987 | KompassStartwert = KompassValue; |
Line 984... | Line 996... | ||
984 | if(w > 0) |
996 | if(w > 0) |
985 | { |
997 | { |
986 | if(!KompassSignalSchlecht) |
998 | if(!KompassSignalSchlecht) |
987 | { |
999 | { |
988 | GierGyroFehler += fehler; |
1000 | GierGyroFehler += fehler; |
- | 1001 | v = 64 + ((MaxStickNick + MaxStickRoll)) / 8; |
|
- | 1002 | //v = 32; |
|
- | 1003 | //r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180; |
|
- | 1004 | r = KompassRichtung; |
|
989 | v = (KompassRichtung * w) / 32; // nach Kompass ausrichten |
1005 | v = (r * w) / v; // nach Kompass ausrichten |
990 | w = Parameter_KompassWirkung; |
1006 | w = 3 * Parameter_KompassWirkung; |
991 | if(v > w) v = w; // Begrenzen |
1007 | if(v > w) v = w; // Begrenzen |
992 | else |
1008 | else |
993 | if(v < -w) v = -w; |
1009 | if(v < -w) v = -w; |
994 | Mess_Integral_Gier += v; |
1010 | Mess_Integral_Gier += v; |
995 | } |
1011 | } |
996 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
1012 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
997 | } |
1013 | } |
998 | else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
1014 | else KompassSignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 0,5 sek |
999 | } |
1015 | } |
1000 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1016 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1001... | Line 1017... | ||
1001 | 1017 | ||
1002 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1018 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1019... | Line 1035... | ||
1019 | DebugOut.Analog[10] = SenderOkay; |
1035 | DebugOut.Analog[10] = SenderOkay; |
1020 | DebugOut.Analog[16] = Mittelwert_AccHoch; |
1036 | DebugOut.Analog[16] = Mittelwert_AccHoch; |
Line 1021... | Line 1037... | ||
1021 | 1037 | ||
1022 | DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
1038 | DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
- | 1039 | DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
|
Line 1023... | Line 1040... | ||
1023 | DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
1040 | DebugOut.Analog[19] = WinkelOut.CalcState; |
1024 | 1041 | ||
Line 1025... | Line 1042... | ||
1025 | DebugOut.Analog[30] = GPS_Nick; |
1042 | DebugOut.Analog[30] = GPS_Nick; |