WebSVN - FlightCtrl - Diff - Rev 395 and 401 - /branches/V0.69k-Bus-Dietmar/fc.c

 #include "eeprom.c"
 unsigned char h,m,s;
 volatile unsigned int I2CTimeout = 100;
 volatile int MesswertNick,MesswertRoll,MesswertGier;
-volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
+volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 volatile float NeutralAccZ = 0;
 unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
 volatile long IntegralNick = 0,IntegralNick2 = 0;
+     }
      AdNeutralNick= AdWertNick;
          AdNeutralRoll= AdWertRoll;
-         AdNeutralGier= AdWertGier;
+         AdNeutralGier= AdWertGier;
+     StartNeutralRoll = AdNeutralRoll;
      StartNeutralNick = AdNeutralNick;
     NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
         NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
         NeutralAccZ = Aktuell_az;
+{
     static signed long tmpl,tmpl2;
     MesswertGier = (signed int) AdNeutralGier - AdWertGier;
     MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
     MesswertNick = (signed int) AdWertNick - AdNeutralNick;
-    if(PlatinenVersion != 10)
-     {
-      MesswertGier *= 2;
-          MesswertRoll *= 2;
-      MesswertNick *= 2;
-     }
 // Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
         Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
         Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
         Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
     IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
              Mess_IntegralRoll =  (Umschlag180Roll - 10000L);
              Mess_IntegralRoll2 = Mess_IntegralRoll;
+            }
             if(AdWertRoll < 15)   MesswertRoll = -1000;
             if(AdWertRoll <  7)   MesswertRoll = -2000;
+            if(PlatinenVersion == 10)
+                         {
-            if(AdWertRoll > 1010) MesswertRoll = +1000;
+              if(AdWertRoll > 1010) MesswertRoll = +1000;
-            if(AdWertRoll > 1017) MesswertRoll = +2000;
+              if(AdWertRoll > 1017) MesswertRoll = +2000;
+                         }
+                         else
+                         {
+              if(AdWertRoll > 2020) MesswertRoll = +1000;
+              if(AdWertRoll > 2034) MesswertRoll = +2000;
+                         }
 // Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
             MesswertNick -= tmpl2;
             MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
             Mess_IntegralNick2 += MesswertNick;
             Mess_IntegralNick  += MesswertNick - I_LageNick;
              Mess_IntegralNick =  (Umschlag180Nick - 10000L);
              Mess_IntegralNick2 = Mess_IntegralNick;
+            }
             if(AdWertNick < 15)   MesswertNick = -1000;
             if(AdWertNick <  7)   MesswertNick = -2000;
+            if(PlatinenVersion == 10)
+                         {
-            if(AdWertNick > 1010) MesswertNick = +1000;
+              if(AdWertNick > 1010) MesswertNick = +1000;
-            if(AdWertNick > 1017) MesswertNick = +2000;
+              if(AdWertNick > 1017) MesswertNick = +2000;
+                         }
+                         else
+                         {
+              if(AdWertNick > 2020) MesswertNick = +1000;
+              if(AdWertNick > 2034) MesswertNick = +2000;
+                         }
 //++++++++++++++++++++++++++++++++++++++++++++++++
 // ADC einschalten
     ANALOG_ON;
 //++++++++++++++++++++++++++++++++++++++++++++++++
+DebugOut.Analog[11] = MesswertRoll;
     Integral_Gier  = Mess_Integral_Gier;
     IntegralNick = Mess_IntegralNick;
     IntegralRoll = Mess_IntegralRoll;
     MittelIntegralNick  /= ABGLEICH_ANZAHL;
     MittelIntegralRoll  /= ABGLEICH_ANZAHL;
         IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
         IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
     IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
-//DebugOut.Analog[6] = labs(MittelIntegralNick_Alt - MittelIntegralNick) / 10;
+#define MAX_I 32
 // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
     ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
     if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)
+     {
        tmp_long = IntegralFehlerNick / 700L;
-       if(tmp_long > 40) tmp_long = 40;
+       if(tmp_long > MAX_I) tmp_long = MAX_I;
-       if(tmp_long <-40) tmp_long =-40;
+       if(tmp_long <-MAX_I) tmp_long =-MAX_I;
        I_LageNick = tmp_long;
+     }
      else
+     {
       I_LageNick /= 2;
     IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
     ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
     if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000)
+     {
       tmp_long2 = IntegralFehlerRoll / 700L;
-      if(tmp_long2 > 40) tmp_long2 = 40;
+      if(tmp_long2 > MAX_I) tmp_long2 = MAX_I;
-      if(tmp_long2 <-40) tmp_long2 =-40;
+      if(tmp_long2 <-MAX_I) tmp_long2 =-MAX_I;
       I_LageRoll = tmp_long2;
+     }
      else
+     {
       I_LageRoll /=2;
     IntegralFehlerNick = tmp_long;
     IntegralFehlerRoll = tmp_long2;
     Mess_IntegralNick2 -= IntegralFehlerNick;
     Mess_IntegralRoll2 -= IntegralFehlerRoll;
-    if(PlatinenVersion != 10)
-    {
-     tmp_long /= 2;
-     tmp_long2 /= 2;
-    }
 //    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
 //    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
-DebugOut.Analog[30] = I_LageRoll;
+DebugOut.Analog[30] = I_LageRoll * 10;
 DebugOut.Analog[17] = IntegralAccNick / 26;
 DebugOut.Analog[18] = IntegralAccRoll / 26;
 DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
 DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
 DebugOut.Analog[21] = MittelIntegralNick / 26;
 DebugOut.Analog[22] = MittelIntegralRoll / 26;
 DebugOut.Analog[28] = ausgleichNick;
 DebugOut.Analog[29] = ausgleichRoll;
 #define FEHLER_LIMIT  (ABGLEICH_ANZAHL/2)
-#define FEHLER_LIMIT2  4096
+#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL*4)
 // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
         cnt = 1;// + labs(IntegralFehlerNick) / 4096;
         if(IntegralFehlerNick >  FEHLER_LIMIT2)
          {
            if(last_n_p)
+           {
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
-            ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
+            ausgleichNick = IntegralFehlerNick / 4;
-            if(ausgleichNick > 10000) ausgleichNick = 10000;
+            if(ausgleichNick > 5000) ausgleichNick = 5000;
-            Mess_IntegralNick += ausgleichNick;
+            Mess_IntegralNick -= ausgleichNick;
+           }
            else last_n_p = 1;
          } else  last_n_p = 0;
         if(IntegralFehlerNick < -FEHLER_LIMIT2)
          {
            if(last_n_n)
+            {
              cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
-             ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
+             ausgleichNick = IntegralFehlerNick / 4;
-             if(ausgleichNick < -10000) ausgleichNick = -10000;
+             if(ausgleichNick < -5000) ausgleichNick = -5000;
-             Mess_IntegralNick += ausgleichNick;
+             Mess_IntegralNick -= ausgleichNick;
+            }
-           else last_n_n = 1;
-         } else  last_n_n = 0;
+           else last_n_n = 1;
          } else  last_n_n = 0;
         if(cnt > 2) cnt = 2;
+        if(cnt > 3) cnt = 3;
         if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
         if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
 //        if(IntegralFehlerNick >  FEHLER_LIMIT)  { AdNeutralNick += (cnt + last_n_p/20);  if(last_n_p++ == 4) {last_n_p = 4;}} else last_n_p = 0;
-//        if(IntegralFehlerNick < -FEHLER_LIMIT)  { AdNeutralNick -= (cnt + last_n_n/20);  if(last_n_n++ == 4) {last_n_n = 4;}} else last_n_n = 0;
+// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
-// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
+        cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
-        cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
+ausgleichRoll = 0;
         if(IntegralFehlerRoll >  FEHLER_LIMIT2)
          {
            if(last_r_p)
+           {
             cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
-            ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
+            ausgleichRoll = IntegralFehlerRoll / 4;
-            if(ausgleichRoll > 10000) ausgleichRoll = 10000;
+            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
-            Mess_IntegralRoll += ausgleichRoll;
+            Mess_IntegralRoll -= ausgleichRoll;
+           }
            else last_r_p = 1;
          } else  last_r_p = 0;
         if(IntegralFehlerRoll < -FEHLER_LIMIT2)
          {
            if(last_r_n)
+           {
             cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
+            ausgleichRoll = IntegralFehlerRoll / 4;
+            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
-            ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
+            Mess_IntegralRoll -= ausgleichRoll;
             if(ausgleichRoll < -10000) ausgleichRoll = -10000;
-            Mess_IntegralRoll += ausgleichRoll;
-           }
-           else last_r_n = 1;
-         } else  last_r_n = 0;
-        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
-        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
-        if(cnt > 2) cnt = 2;
+           else last_r_n = 1;
-//        if(IntegralFehlerRoll >  FEHLER_LIMIT)  { AdNeutralRoll += (cnt + last_r_p/20);  if(last_r_p++ == 4) {beeptime =500;last_r_p = 4;}} else last_r_p = 0;
+         } else  last_r_n = 0;
 //        if(IntegralFehlerRoll < -FEHLER_LIMIT)  { AdNeutralRoll -= (cnt + last_r_n/20);  if(last_r_n++ == 4) {beeptime = 60;last_r_n = 4;}} else last_r_n = 0;
 DebugOut.Analog[27] = ausgleichRoll;
-//        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)
+        if(cnt > 3) cnt = 3;
-//        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;   // macht nur mit Referenz (Kompass Sinn)
+        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;

Subversion Repositories FlightCtrl

(root)/branches/V0.69k-Bus-Dietmar/fc.c @ 1341 - Rev 395 → 401