WebSVN - FlightCtrl - Diff - Rev 566 and 579 - /branches/salvo_gps/Basis

 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
     if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
 //Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen
 // und dieser dann langsam zwangsweise reduziert
-        if (UBat <= EE_Parameter.UnterspannungsWarnung - 2)     //Unterhalb der Piepser Schwelle aktivieren
+        if (UBat <= EE_Parameter.UnterspannungsWarnung - 4)     //Unterhalb der Piepser Schwelle aktivieren
+        {
                 if (ubat_cnt > 1000)
+                {
                         ubat_cnt = 0;
                         if (gas_actual > ((gas_mittel*12)/15)) gas_actual--;
              LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
+            }
            else last_n_n = 1;
          } else  last_n_n = 0;
         } else cnt = 0;
-//Salvo 11.12.2007
+//Salvo 26.12.2007
                 w = (abs(Mittelwert_AccNick));
                 v = (abs(Mittelwert_AccRoll));
-                if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in wwagrechter Lage nachtrimmen
+                if  ((w  < ACC_WAAGRECHT_LIMIT*2) && (v < ACC_WAAGRECHT_LIMIT*2)) // Gyro nur in annaehend waagrechter Lage nachtrimmen
+                {
                 if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
                 if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
                 if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
+                }
          } else  last_r_n = 0;
         } else
+        {
          cnt = 0;
+        }
-//Salvo 11.12.2007
+//Salvo 26.12.2007
-                w = (abs(Mittelwert_AccNick));
-                v = (abs(Mittelwert_AccRoll));
-                if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in wwagrechter Lage nachtrimmen
+                if  ((w  < ACC_WAAGRECHT_LIMIT*2) && (v < ACC_WAAGRECHT_LIMIT*2)) // Gyro nur inannaehernd waagrechter Lage nachtrimmen
+                {
                 if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
                 if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
                 if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
-             }
+            }
 //Salvo End
 //DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
 //DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
+  }
   else
   if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++
    MittelIntegralNick_Alt = MittelIntegralNick;
    MittelIntegralRoll_Alt = MittelIntegralRoll;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++
-    IntegralAccNick = 0;
+   IntegralAccNick = 0;
-    IntegralAccRoll = 0;
+   IntegralAccRoll = 0;
-    IntegralAccZ = 0;
+   IntegralAccZ = 0;
-    MittelIntegralNick = 0;
+   MittelIntegralNick = 0;
-    MittelIntegralRoll = 0;
+   MittelIntegralRoll = 0;
-    MittelIntegralNick2 = 0;
+   MittelIntegralNick2 = 0;
-    MittelIntegralRoll2 = 0;
+   MittelIntegralRoll2 = 0;
-    ZaehlMessungen = 0;
+   ZaehlMessungen = 0;
  } // Ende Abgleich
+// Salvo Ersatzkompass  und Giergyrokompensation 15.12.2007 **********************
+if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat
-        // Salvo Ersatzkompass  und Giergyrokompensation 15.12.2007 **********************
+{
+           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 ((Kompass_Neuer_Wert > 0))
+        if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
-        {
-           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 ((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
+                        if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da
-                                {
+                        {
-                                        w = (int) (GyroGier_Comp/(long)GYROKOMP_INC_GRAD_DEFAULT);
+                                w = (int) (GyroGier_Comp/(long)GYROKOMP_INC_GRAD_DEFAULT);
-                                        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
-                                        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
-                                        #define GIER_COMP_MAX 4
+                                #define GIER_COMP_MAX 8
-                                        if (v > GIER_COMP_MAX)  v= GIER_COMP_MAX;
+                                if (v > GIER_COMP_MAX)  v= GIER_COMP_MAX;
-                                        if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
+                                if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
-                                        if (abs(w) > 1)
+                                if (abs(w) > 1)
-                                        {
+                                {
-                                                GyroGier_Comp           = 0;
+                                        GyroGier_Comp           = 0;
-                                                gyrogier_kompass        = KompassValue; // Kompasswert merken
-                                                AdNeutralGier           -= v;
+                                        gyrogier_kompass        = KompassValue; // Kompasswert merken
                                         AdNeutralGier           -= v;
+                                }
-                                }
+                        }
-                        }
+          }
-                  else
+          else
-                  {
+          {
-                        gyrogier_kompass        = KompassValue; // Kompasswert merken
+                gyrogier_kompass        = KompassValue; // Kompasswert merken
-                        cnt_stickgier_zero      = 0;
+                cnt_stickgier_zero      = 0;
-                        GyroGier_Comp = 0;
+                GyroGier_Comp = 0;
-                  }
+          }
-                  magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
+          magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
-                  GyroKomp_Int = (GyroKomp_Int )/(long)GYROKOMP_INC_GRAD_DEFAULT;
+          GyroKomp_Int = (GyroKomp_Int )/(long)GYROKOMP_INC_GRAD_DEFAULT;
-                  w = KompassValue - GyroKomp_Int;
+          w = KompassValue - GyroKomp_Int;
-                  if ((w > 0) && (w < 180))
+          if ((w > 0) && (w < 180))
-                  {
+          {
-                   ++GyroKomp_Int;
+           ++GyroKomp_Int;
-                  }
+          }
-                  else if ((w > 0) && (w >= 180))
+          else if ((w > 0) && (w >= 180))
-                  {
+          {
-                   --GyroKomp_Int;
+           --GyroKomp_Int;
-                  }
+          }
-                  else if ((w < 0) && (w >= -180))
+          else if ((w < 0) && (w >= -180))
-                  {
+          {
-                   --GyroKomp_Int;
+           --GyroKomp_Int;
-                  }
+          }
-                  else if ((w < 0) && (w < -180))
+          else if ((w < 0) && (w < -180))
-                  {
+          {
-                   ++GyroKomp_Int;
+           ++GyroKomp_Int;
-                  }
+          }
-                  if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360L;
+          if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360L;
-                  GyroKomp_Int = (GyroKomp_Int%360L) * (long)GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
+          GyroKomp_Int = (GyroKomp_Int%360L) * (long)GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
-                 }
+        }
-           }
+   }
-           else
+   else //Kompassfehler
-           {
+   {
-            magkompass_ok = 0;
+    magkompass_ok = 0;
-                GyroGier_Comp = 0;
+        GyroGier_Comp = 0;
-      }
+   }
-        Kompass_Value_Old       =       KompassValue;
+   Kompass_Value_Old    =       KompassValue;
+}
 // Salvo End *************************
         DebugOut.Analog[25] =  GPS_Roll;
         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[30] =  GPS_dist_2trgt;
+        DebugOut.Analog[30] =  dist_flown;
+        DebugOut.Analog[31] =  (int) dist_2home;
+//      DebugOut.Analog[31] =  (int) GPS_hdng_abs_2trgt;
-        DebugOut.Analog[31] =  (int) GyroGier_Comp;
+//      DebugOut.Analog[31] =  (int) GyroGier_Comp;
 /*    DebugOut.Analog[16] = motor_rx[0];
     DebugOut.Analog[17] = motor_rx[1];
     DebugOut.Analog[18] = motor_rx[2];
     DebugOut.Analog[19] = motor_rx[3];
     DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/fc.c @ 582 - Rev 566 → 579