WebSVN - FlightCtrl - Diff - Rev 123 and 124 - /branches/salvo

 volatile int  KompassRichtung = 0;
 unsigned char MAX_GAS,MIN_GAS;
 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;
 volatile int  DiffNick,DiffRoll;
     HoeheD = 0;
     Mess_Integral_Hoch = 0;
     KompassStartwert = KompassValue;
     GPS_Neutral();
     beeptime = 50;
+//Salvo 2.9.2007 Ersatzkompass
+        GyroKomp_Int = 0;
+        GyroKomp_Int2 = 0;
+// Salvo End
+}
 //############################################################################
 // Bildet den Mittelwert aus den Messwerten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gyro-Drift kompensieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define DRIFT_FAKTOR 3
     if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
-        {
+                {
-        IntegralFehlerNick = IntegralNick2 - IntegralNick;
+               IntegralFehlerNick = IntegralNick2 - IntegralNick;
-        IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
+           IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
-        ZaehlMessungen = 0;
+           ZaehlMessungen = 0;
 // Salvo 1.9.2007 *************************
-// Abgleich Roll und Nick Gyro nur, wenn nahezu waagrechte Lage
+// 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
+                   ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
-                w = (abs(Mittelwert_AccNick));
+                        w = (abs(Mittelwert_AccNick));
-                v = (abs(Mittelwert_AccRoll));
+                        v = (abs(Mittelwert_AccRoll));
-        ANALOG_ON;      // ADC einschalten
+                ANALOG_ON;      // ADC einschalten
-                if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
+                        if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
-                {
+                        {
-         if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
+                 if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
-         if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
+                 if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
-         if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
+                 if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
-         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 ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht))
-                {
+                        {
-         if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;
+                 if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;
-         if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;
+                 if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;
-                }
+                        }
-                else
+                        else
-                {
+                        {
-                  Mess_Integral_Gier2 = 0;
+                         Mess_Integral_Gier2 = 0;
+                        }
 // Salvo End ***********************
-    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
+                        ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
-        Mess_IntegralNick2 = IntegralNick;
+                Mess_IntegralNick2 = IntegralNick;
-        Mess_IntegralRoll2 = IntegralRoll;
+                Mess_IntegralRoll2 = IntegralRoll;
-        Mess_Integral_Gier2 = Integral_Gier;
+                Mess_Integral_Gier2 = Integral_Gier;
-    ANALOG_ON;  // ADC einschalten
+                ANALOG_ON;      // ADC einschalten
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Integrale auf ACC-Signal abgleichen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         Mess_IntegralNick -= tmp_long;
         Mess_IntegralRoll -= tmp_long2;
+        }
         else if ((w  < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT))
+        {
-        Mess_IntegralNick -= tmp_long/8;
+        Mess_IntegralNick -= tmp_long/2; //Vorher 8
-        Mess_IntegralRoll -= tmp_long/8;
+        Mess_IntegralRoll -= tmp_long2/2;
+        }
         else if ((w  < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT))
+        {
-        Mess_IntegralNick -= tmp_long/16;
+        Mess_IntegralNick -= tmp_long/4;
-        Mess_IntegralRoll -= tmp_long/16;
+        Mess_IntegralRoll -= tmp_long2/4;
+        }
         else
+        {
-        Mess_IntegralNick -= tmp_long/32;
+        Mess_IntegralNick -= tmp_long/8;
-        Mess_IntegralRoll -= tmp_long2/32;
+        Mess_IntegralRoll -= tmp_long2/8;
+        }
 // Salvo End ***********************
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[5] = HoehenWert;
     DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
     DebugOut.Analog[7] = GasMischanteil;
     DebugOut.Analog[8] = KompassValue;
-    DebugOut.Analog[9] = tmp_long;
+    DebugOut.Analog[9] = GyroKomp_Int;
-    DebugOut.Analog[10] = tmp_long2;
+    DebugOut.Analog[10] = GyroKomp_Int2;
 //    DebugOut.Analog[9] = SollHoehe;
 //    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 //    DebugOut.Analog[11] = KompassStartwert;
 //    DebugOut.Analog[10] = Parameter_Gyro_I;
 //    DebugOut.Analog[10] = EE_Parameter.Gyro_I;

Subversion Repositories FlightCtrl

(root)/branches/salvo_gierkompass/fc.c @ 1042 - Rev 123 → 124