WebSVN - FlightCtrl - Diff - Rev 1093 and 1096 - /branches/salvo_gps/Basis

     NaviAccRoll    += AdWertAccRoll;
     NaviCntAcc++;
     IntegralAccZ    += Aktuell_az - NeutralAccZ;
 // Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
             ErsatzKompass +=  MesswertGier;
-//Salvo 12.11.2007
+//Salvo 29.12.2008
+        if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
+        {
                         GyroKomp_Int   += (long)MesswertGier;
                         GyroGier_Comp  += (long)MesswertGier;
+        }
 //Salvo End
             Mess_Integral_Gier +=  MesswertGier;
 //            Mess_Integral_Gier2 += MesswertGier;
                     if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag
                     if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
     Mess_IntegralNick2 -= IntegralFehlerNick;
     Mess_IntegralRoll2 -= IntegralFehlerRoll;
 //    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
+//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
-//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
+//Salvo 29.12.2008 auskommentiert
-    if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
+//    if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
+//    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
-    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
+//Salvo End
 //DebugOut.Analog[22] = MittelIntegralRoll / 26;
+ }
 //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
 // Salvo Ersatzkompass  und Giergyrokompensation 15.12.2007 **********************
 if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat
+{
-           Kompass_Neuer_Wert = 0;
+   Kompass_Neuer_Wert = 0;
    w = (abs(Mittelwert_AccNick));
    v = (abs(Mittelwert_AccRoll));
    if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok
+   {
         if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
+        {
           if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass
+          {
                         if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;
-                        if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da
+                        if (cnt_stickgier_zero > 10) // nur Abgleichen wenn keine Stickbewegung da
+                        {
                                 w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR);
                                 v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
                                 if (v <-180) v +=360; // Uberlaufkorrektur
     magkompass_ok = 0;
         GyroGier_Comp = 0;
+   }
    Kompass_Value_Old    =       KompassValue;
+}
+else  magkompass_ok = 0;
 // Salvo End *************************
 // Salvo 6.10.2007
         // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //    if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
     if(abs(StickGier) > 15) // war 35
+     {
       KompassSignalSchlecht = 1000;
-      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
+      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
-       {
-         NeueKompassRichtungMerken = 1;
-        };
+     }
     tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
     tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
     sollGier = tmp_int;
     Mess_Integral_Gier -= tmp_int;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
-// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
+// Salvo 29.12.2008
-                if ((magkompass_ok > 0)  &&  NeueKompassRichtungMerken)
+                if (NeueKompassRichtungMerken)
+        {
                  KompassStartwert = KompassValue;
          NeueKompassRichtungMerken = 0;
+        }
        w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
        if(w >= 0)
+        {
 // Salvo Kompasssteuerung **********************
-                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
+                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten wenn dieser ok ist
 // Salvo End
+        }
     DebugOut.Analog[25] = debug_gp_2;
         DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben
         DebugOut.Analog[27] = gps_rel_act_position.utm_north;
         DebugOut.Analog[28] = gps_rel_act_position.utm_alt;
         DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd);
+//      DebugOut.Analog[29] = magkompass_ok;
     DebugOut.Analog[30] = GPS_Nick;
+    DebugOut.Analog[31] = GPS_Roll;
+//    DebugOut.Analog[30] = KompassStartwert;
-    DebugOut.Analog[31] = GPS_Roll;
+//    DebugOut.Analog[31] = KompassRichtung;

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_V0071h/trunk/fc.c - Rev 1093 → 1096