WebSVN - FlightCtrl - Diff - Rev 1102 and 1103 - /branches/salvo_gps/Basis

 //Salvo 12.10.2007
 uint8_t magkompass_ok=0;
 uint8_t gps_cmd = GPS_CMD_STOP;
 static int       ubat_cnt =0;
 static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung
-int w,v;
+int w,v,w1,v1;
 //Salvo End
  //Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation
 long GyroKomp_Int;
 struct mk_param_struct EE_Parameter;
 signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 int MaxStickNick = 0,MaxStickRoll = 0;
 unsigned int  modell_fliegt = 0;
 unsigned char MikroKopterFlags = 0;
+//Salvo 31.12.2008
+long waagrecht; // Mass fuer Annaeherung an Waagrecht fuer Kompasskompensation
+// Salvo End
 //Salvo 2.1.2008 Allgemeine Debugvariablen
 int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7;
 if ((Kompass_Neuer_Wert > 0) && (Kompass_present>0)) //nur wenn Kompass einen neuen Wert geliefert hat
+{
    Kompass_Neuer_Wert = 0;
    w = (abs(Mittelwert_AccNick));
    v = (abs(Mittelwert_AccRoll));
+   w1 = abs((int)(IntegralNick / EE_Parameter.GyroAccFaktor)); //Abfrage auch gegen Gyro  beiu Kurvenflug sonst evtl. vorgetäuschte waagrechte Lage
+   v1 = abs((int)(IntegralRoll / EE_Parameter.GyroAccFaktor));
-   if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok
+   if  ((w1<ACC_WAAGRECHT_LIMIT)&&(w<ACC_WAAGRECHT_LIMIT)&&(v<ACC_WAAGRECHT_LIMIT)&&(v1<ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok
+   {
+   waagrecht = ((long)(ACC_WAAGRECHT_LIMIT-(long)w) * ((long)ACC_WAAGRECHT_LIMIT-(long)w)) *(((long)ACC_WAAGRECHT_LIMIT-(long)v)* ((long)ACC_WAAGRECHT_LIMIT-(long)v));
+   waagrecht =  (waagrecht * 100) /((long)ACC_WAAGRECHT_LIMIT * (long)ACC_WAAGRECHT_LIMIT * (long)ACC_WAAGRECHT_LIMIT*(long)ACC_WAAGRECHT_LIMIT) ; //bei Waagrecht  Wert = 100
         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 > 1) //nur Abgleichen wenn keine Stickbewegung da
+                        {
-                                w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR);
+                                w = (int) (GyroGier_Comp/((long)GIER_GRAD_FAKTOR));
-                                v = KompassValue - gyrogier_kompass; //realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
+                                v = (KompassValue) - gyrogier_kompass; //realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
                                 if (v <-180) v +=360; // Uberlaufkorrektur
                                 if (v > 180) v -=360; // Uberlaufkorrektur
+                                v = w-v;  //Differenz Gyro zu Kompass ist der Driftfehler
-                                v = w-v;  //Differenz Gyro zu Kompass ist der Driftfehler
+                                v = (v * (int) waagrecht)/100; //mit Neigung bewerten
                                 if (v > GIER_COMP_MAX)  v= GIER_COMP_MAX;
                                 if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
-                                if (abs(w) > 1)
+                                if (abs(v) > 0)
+                                {
                                         GyroGier_Comp           = 0;
                                         gyrogier_kompass        = KompassValue; // Kompasswert merken
                 cnt_stickgier_zero      = 0;
                 GyroGier_Comp = 0;
+          }
           magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
-          GyroKomp_Int = (GyroKomp_Int )/(long)GIER_GRAD_FAKTOR;
+//        GyroKomp_Int = (GyroKomp_Int )/((long)GIER_GRAD_FAKTOR);
-          w = KompassValue - GyroKomp_Int;
-          if ((w > 0) && (w < 180))
+          w = KompassValue - (int)(GyroKomp_Int/(long)GIER_GRAD_FAKTOR);
+          w = w%360;
+          #define GYRO_KOMP_DELTA_MAX 10
-          {
+          if (w > GYRO_KOMP_DELTA_MAX)  w = GYRO_KOMP_DELTA_MAX; //Begrenzung
-           ++GyroKomp_Int;
+          if (w <-GYRO_KOMP_DELTA_MAX)  w =-GYRO_KOMP_DELTA_MAX; //Begrenzung
-          else if ((w > 0) && (w >= 180))
+          if (((w > 0) && (w < 180)) || ((w < 0) && (w < -180)))
+          {
-           --GyroKomp_Int;
+           GyroKomp_Int = GyroKomp_Int + (((labs((long)w) * waagrecht)/100)*(long)GIER_GRAD_FAKTOR); // je waagrechter desto groesser die Korrektur
+          }
-          else if ((w < 0) && (w >= -180))
+          else if (((w > 0) && (w >= 180)) || ((w < 0) && (w >= -180)))
+          {
-           --GyroKomp_Int;
+           GyroKomp_Int = GyroKomp_Int - (((labs((long)w) * waagrecht)/100)*(long)GIER_GRAD_FAKTOR);
-          }
-          else if ((w < 0) && (w < -180))
-          {
-           ++GyroKomp_Int;
+          }
-          }
+          if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360L * (long)GIER_GRAD_FAKTOR; //Ueberlauf korrigieren
-          if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360L;
+          if (GyroKomp_Int > 360L * (long)GIER_GRAD_FAKTOR)  GyroKomp_Int = GyroKomp_Int - 360L * (long)GIER_GRAD_FAKTOR;
-          GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern
+//        GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern
+        }
+   }
    else //Kompass wegen zu grosser Neigung ungueltig
     DebugOut.Analog[30] = GPS_Nick;
     DebugOut.Analog[31] = GPS_Roll;
 //    DebugOut.Analog[30] = KompassStartwert;
+//    DebugOut.Analog[31] = KompassRichtung;
+//    DebugOut.Analog[31] = (int) waagrecht;
 //    DebugOut.Analog[31] = KompassRichtung;

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_V0071h/trunk/fc.c @ 1865 - Rev 1102 → 1103