| 76,7 → 76,9 |
|---|
| unsigned char Notlandung = 0; |
| unsigned char HoehenReglerAktiv = 0; |
| static int SignalSchlecht = 0; |
| |
| //Salvo 2.9.2007 Ersatzkompass |
| volatile long GyroKomp_Int,GyroKomp_Int2; |
| // Salvo End |
| float GyroFaktor; |
| float IntegralFaktor; |
| |
| 170,6 → 172,10 |
|---|
| KompassStartwert = KompassValue; |
| GPS_Neutral(); |
| beeptime = 50; |
| //Salvo 2.9.2007 Ersatzkompass |
| GyroKomp_Int = 0; |
| GyroKomp_Int2 = 0; |
| // Salvo End |
| } |
| |
| //############################################################################ |
| 598,43 → 604,57 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #define DRIFT_FAKTOR 3 |
| if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
| { |
| IntegralFehlerNick = IntegralNick2 - IntegralNick; |
| IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
| ZaehlMessungen = 0; |
| { |
| IntegralFehlerNick = IntegralNick2 - IntegralNick; |
| IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
| ZaehlMessungen = 0; |
| // Salvo 1.9.2007 ************************* |
| // Abgleich Roll und Nick Gyro nur, wenn nahezu waagrechte Lage |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| w = (abs(Mittelwert_AccNick)); |
| v = (abs(Mittelwert_AccRoll)); |
| ANALOG_ON; // ADC einschalten |
| if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) |
| { |
| if(IntegralFehlerNick > 500/DRIFT_FAKTOR) AdNeutralNick++; |
| if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
| if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
| if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
| } |
| // Abgleich Roll und Nick Gyro vollsteandig nur, wenn nahezu waagrechte Lage, ansonsten abschwaechen |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| w = (abs(Mittelwert_AccNick)); |
| v = (abs(Mittelwert_AccRoll)); |
| ANALOG_ON; // ADC einschalten |
| if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) |
| { |
| if(IntegralFehlerNick > 500/DRIFT_FAKTOR) AdNeutralNick++; |
| if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
| if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
| if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
| } |
| else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht |
| { |
| if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR) AdNeutralNick++; |
| if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR) AdNeutralNick--; |
| if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR) AdNeutralRoll++; |
| if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR) AdNeutralRoll--; |
| } |
| else // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht |
| { |
| if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR) AdNeutralNick++; |
| if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR) AdNeutralNick--; |
| if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR) AdNeutralRoll++; |
| if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR) AdNeutralRoll--; |
| } |
| // Salvo End |
| |
| // Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist *********************** |
| // Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert |
| // Ohne Driftkompensation ist die Gierachse wesentlich stabiler |
| if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht)) |
| { |
| if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; |
| if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; |
| } |
| else |
| { |
| Mess_Integral_Gier2 = 0; |
| } |
| if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht)) |
| { |
| if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; |
| if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; |
| } |
| else |
| { |
| Mess_Integral_Gier2 = 0; |
| } |
| // Salvo End *********************** |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| Mess_IntegralNick2 = IntegralNick; |
| Mess_IntegralRoll2 = IntegralRoll; |
| Mess_Integral_Gier2 = Integral_Gier; |
| ANALOG_ON; // ADC einschalten |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| Mess_IntegralNick2 = IntegralNick; |
| Mess_IntegralRoll2 = IntegralRoll; |
| Mess_Integral_Gier2 = Integral_Gier; |
| ANALOG_ON; // ADC einschalten |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Integrale auf ACC-Signal abgleichen |
| 658,18 → 678,18 |
|---|
| } |
| else if ((w < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT)) |
| { |
| Mess_IntegralNick -= tmp_long/8; |
| Mess_IntegralRoll -= tmp_long/8; |
| Mess_IntegralNick -= tmp_long/2; //Vorher 8 |
| Mess_IntegralRoll -= tmp_long2/2; |
| } |
| else if ((w < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT)) |
| { |
| Mess_IntegralNick -= tmp_long/16; |
| Mess_IntegralRoll -= tmp_long/16; |
| Mess_IntegralNick -= tmp_long/4; |
| Mess_IntegralRoll -= tmp_long2/4; |
| } |
| else |
| { |
| Mess_IntegralNick -= tmp_long/32; |
| Mess_IntegralRoll -= tmp_long2/32; |
| Mess_IntegralNick -= tmp_long/8; |
| Mess_IntegralRoll -= tmp_long2/8; |
| } |
| // Salvo End *********************** |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 746,8 → 766,9 |
|---|
| DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
| DebugOut.Analog[7] = GasMischanteil; |
| DebugOut.Analog[8] = KompassValue; |
| DebugOut.Analog[9] = tmp_long; |
| DebugOut.Analog[10] = tmp_long2; |
| DebugOut.Analog[9] = GyroKomp_Int; |
| DebugOut.Analog[10] = GyroKomp_Int2; |
| |
| // DebugOut.Analog[9] = SollHoehe; |
| // DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
| // DebugOut.Analog[11] = KompassStartwert; |