WebSVN - FlightCtrl - Diff - Rev 1837 and 1839 - /branches/Proxy sensor

 long SummeNick=0,SummeRoll=0;
 volatile long Mess_Integral_Hoch = 0;
 int  KompassValue = 0;
 int  KompassSollWert = 0;
 int  KompassRichtung = 0;
+char CalculateCompassTimer = 100;
+unsigned char KompassFusion = 32;
-unsigned int  KompassSignalSchlecht = 500;
+unsigned int  KompassSignalSchlecht = 50;
 unsigned char  MAX_GAS,MIN_GAS;
 unsigned char HoehenReglerAktiv = 0;
 unsigned char TrichterFlug = 0;
 long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 long  ErsatzKompass;
     DebugOut.Analog[20] = ServoNickValue;
     DebugOut.Analog[22] = Capacity.ActualCurrent;
     DebugOut.Analog[23] = Capacity.UsedCapacity;
 //    DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;
 //    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
+    DebugOut.Analog[27] = KompassSollWert;
         DebugOut.Analog[29] = Capacity.MinOfMaxPWM;
     DebugOut.Analog[30] = GPS_Nick;
     DebugOut.Analog[31] = GPS_Roll;
     if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe;
     Mess_Integral_Gier = 0;
     StartLuftdruck = Luftdruck;
     VarioMeter = 0;
     Mess_Integral_Hoch = 0;
     KompassSollWert = KompassValue;
+        KompassSignalSchlecht = 100;
     GPS_Neutral();
     beeptime = 50;
         Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
         Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
     ExternHoehenValue = 0;
 // Trägt ggf. das Poti als Parameter ein
 void ParameterZuordnung(void)
 //############################################################################
+{
  unsigned char tmp,i;
+ static unsigned char carefree_old = 2;
  #define CHK_POTI(b,a) {if(a < 248) b = a; else b = Poti[255 - a];}
  #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max);}
   for(i=0;i<8;i++)
+    {
      int tmp2;
  tmp = EE_Parameter.OrientationModeControl;
  if(tmp > 50)
+   {
 #ifdef SWITCH_LEARNS_CAREFREE
-    if(!CareFree) ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
+    if(!CareFree)  ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
 #endif
         CareFree = 1;
+    if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0;
+    if(carefree_old == 0 && CareFree)  beeptime = 1000;
+    if(carefree_old == 1 && !CareFree) beeptime = 200;
-    if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0;
+    carefree_old = CareFree;
-    if(FromNaviCtrl.CompassValue < 0 && CareFree) VersionInfo.HardwareError[0] |= FC_ERROR0_CAREFREE; else VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE;
+        if(FromNaviCtrl.CompassValue < 0 && CareFree) VersionInfo.HardwareError[0] |= FC_ERROR0_CAREFREE; else VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE;
+   }
+   else
-   }
+   {
+    CareFree = 0;
+        carefree_old = 0;
    else CareFree = 0;
  if(FromNaviCtrl.CompassValue < 0 && MotorenEin && CareFree && BeepMuster == 0xffff) // ungültiger Kompasswert
+        {
+                {
                 SummeNick = 0;
                 SummeRoll = 0;
                 sollGier = 0;
                 Mess_Integral_Gier = 0;
-                if(modell_fliegt == 250)
-                 {
-                  NeueKompassRichtungMerken = 1;
+                NeueKompassRichtungMerken = 250;
-                 }
                 } else FC_StatusFlags |= FC_STATUS_FLY;
             if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin))
+  {
    long tmp_long, tmp_long2;
-   if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/)
+   if(FromNaviCtrl_Value.Kalman_K > 0 /*&& !TrichterFlug*/)
+     {
       tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
       tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
       tmp_long  = (tmp_long  * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
+      tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
-      tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
+          KompassFusion = FromNaviCtrl_Value.Kalman_K;
      if((MaxStickNick > 64) || (MaxStickRoll > 64))
+      {
       tmp_long  /= 2;
       tmp_long2 /= 2;
      if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
+      {
       tmp_long  /= 3;
       tmp_long2 /= 3;
+      }
+      KompassFusion = 25;
 #define AUSGLEICH  32
       if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
       if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
       if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
       if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
          } else  last_n_n = 0;
+        }
         else
+        {
          cnt = 0;
-         KompassSignalSchlecht = 1000;
+         KompassSignalSchlecht = 100;
+        }
         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
                 if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
         if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
         if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
            else last_r_n = 1;
          } else  last_r_n = 0;
         } else
+        {
          cnt = 0;
-         KompassSignalSchlecht = 1000;
+         KompassSignalSchlecht = 100;
+        }
         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
                 if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
         if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
         if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(abs(StickGier) > 15) // war 35
+     {
-      KompassSignalSchlecht = 1000;
+//      KompassSignalSchlecht = 1000;
       if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
+       {
-         NeueKompassRichtungMerken = 1;
+         NeueKompassRichtungMerken = 250;
         };
+     }
     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;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+     {
+      if(CalculateCompassTimer-- == 1)
+          {
-       int w,v,r,fehler,korrektur;
+       int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt
+       CalculateCompassTimer = 13; // falls keine Navi-Daten
+// max. Korrekturwert schätzen
        w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
        v = abs(IntegralRoll /512);
        if(v > w) w = v; // grösste Neigung ermitteln
-       korrektur = w / 8 + 2;
+       korrektur = w / 4 + 1;
+// Kompassfehlerwert bestimmen
        fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
-           //fehler += MesswertGier / 12;
+// GIER_GRAD_FAKTOR ist ca. 1200
 // Kompasswert einloggen
        if(!KompassSignalSchlecht && w < 25)
+        {
         GierGyroFehler += fehler;
         if(NeueKompassRichtungMerken)
+         {
+          if(--NeueKompassRichtungMerken == 0)
-         {
+                   {
-                  ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
+                    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
-          KompassSollWert = (ErsatzKompass/GIER_GRAD_FAKTOR);
+            KompassSollWert = (ErsatzKompass/GIER_GRAD_FAKTOR);
           NeueKompassRichtungMerken = 0;
+         }
+        }
-        }
+// Kompass fusionieren
+       if(!KompassSignalSchlecht) ErsatzKompass += (fehler * KompassFusion) / korrektur;
+DebugOut.Analog[16] = fehler;
+DebugOut.Analog[17] = korrektur;
+DebugOut.Analog[18] = KompassFusion;
+DebugOut.Analog[19] = KompassSignalSchlecht;
+DebugOut.Analog[24] = FromNaviCtrl_Value.Kalman_K;
+// MK drehen
+    if(KompassSignalSchlecht) KompassSignalSchlecht--;
+    else
+        if(!NeueKompassRichtungMerken)
+       {
+           r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassSollWert) % 360) - 180;
+           v = r * (Parameter_KompassWirkung/2);  // nach Kompass ausrichten
+                   Mess_Integral_Gier += v;
+DebugOut.Analog[25] = v;
+//DebugOut.Analog[26] = r;
+       }
-       ErsatzKompass += (fehler * 16) / korrektur;
+/*
        w = (w * Parameter_KompassWirkung) / 32;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
        if(w >= 0)
+        {
           if(!KompassSignalSchlecht)
+          {
+           v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
-           v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
+DebugOut.Analog[16] = korrektur;
            r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassSollWert) % 360) - 180;
            v = (r * w) / v;  // nach Kompass ausrichten
            w = 3 * Parameter_KompassWirkung;
            if(v > w) v = w; // Begrenzen
            else
            if(v < -w) v = -w;
-           Mess_Integral_Gier += v;
+//           Mess_Integral_Gier += v;
+          }
           if(KompassSignalSchlecht) KompassSignalSchlecht--;
+        }
+        else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
+*/
-        else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
+      } // CalculateCompassTimer
-     }
+     }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Subversion Repositories FlightCtrl

(root)/branches/Proxy sensor - tempolo/fc.c @ 2291 - Rev 1837 → 1839