WebSVN - FlightCtrl - Diff - Rev 419 and 469

     ZaehlMessungen = 0;
     I_LageNick = 0;
     I_LageRoll = 0;
+  }
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  if(IntegralFaktor && !Looping_Nick && !Looping_Roll)
+  {
+   long tmp_long, tmp_long2;
+    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
+    tmp_long /= 16;
+    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
+    tmp_long2 /= 16;
+ #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;
+    Mess_IntegralNick -= tmp_long;
+    Mess_IntegralRoll -= tmp_long2;
+  }
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(ZaehlMessungen >= ABGLEICH_ANZAHL)
+ {
   static int cnt = 0;
   static char last_n_p,last_n_n,last_r_p,last_r_n;
     MittelIntegralNick  /= ABGLEICH_ANZAHL;
     MittelIntegralRoll  /= ABGLEICH_ANZAHL;
         IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
         IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
     IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
-#define MAX_I 32
+#define MAX_I 1//(Poti2/10)
 // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
     ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
     if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)
+     {
+     }
      else
+     {
       I_LageRoll /=2;
+     }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++
    MittelIntegralNick_Alt = MittelIntegralNick;
    MittelIntegralRoll_Alt = MittelIntegralRoll;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++
     Mess_IntegralNick -= ausgleichNick;
 //    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
-//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
 //    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
 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[22] = MittelIntegralRoll / 26;
+//DebugOut.Analog[28] = ausgleichNick;
-DebugOut.Analog[28] = ausgleichNick;
+DebugOut.Analog[29] = ausgleichRoll;
-DebugOut.Analog[29] = ausgleichRoll;
+DebugOut.Analog[30] = I_LageRoll * 10;
         if(IntegralFehlerNick >  FEHLER_LIMIT2)
+         {
            if(last_n_p)
+           {
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
-            ausgleichNick = IntegralFehlerNick / 8;
+            ausgleichNick = IntegralFehlerNick / 12;
             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 / 8;
+             ausgleichNick = IntegralFehlerNick / 12;
              if(ausgleichNick < -5000) ausgleichNick = -5000;
-             Mess_IntegralNick -= ausgleichNick;
+//             Mess_IntegralNick -= ausgleichNick;
+            }
            else last_n_n = 1;
          } else  last_n_n = 0;
-        if(cnt > 3) cnt = 3;
+        if(cnt > 5) cnt = 5;
         if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
         if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
         if(IntegralFehlerRoll >  FEHLER_LIMIT2)
+         {
            if(last_r_p)
+           {
             cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
-            ausgleichRoll = IntegralFehlerRoll / 4;
+            ausgleichRoll = IntegralFehlerRoll / 12;
             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;
+            ausgleichRoll = IntegralFehlerRoll / 12;
             if(ausgleichRoll < -5000) ausgleichRoll = -5000;
-            Mess_IntegralRoll -= ausgleichRoll;
+//            Mess_IntegralRoll -= ausgleichRoll;
+           }
            else last_r_n = 1;
          } else  last_r_n = 0;
 DebugOut.Analog[27] = ausgleichRoll;
-        if(cnt > 3) cnt = 3;
+        if(cnt > 5) cnt = 5;
         if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
         if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
+DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
 DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
+  }
     IntegralAccNick = 0;
     IntegralAccRoll = 0;
     IntegralAccZ = 0;
     MittelIntegralNick = 0;
     MittelIntegralRoll = 0;
     MittelIntegralNick2 = 0;
     MittelIntegralRoll2 = 0;
     ZaehlMessungen = 0;
+ }
 //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
     if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
     else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 //    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
     MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
+DebugOut.Analog[28] = MesswertRoll;
+DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
+DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
     // Maximalwerte abfangen
     #define MAX_SENSOR  2048
     if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
     if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
     if(GasMischanteil < 20) GierMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
+    if(IntegralRoll) SummeNick += IntegralNick * IntegralFaktor - StickNick;
-    SummeNick += DiffNick;                                   // I-Anteil
+    else  SummeNick += DiffNick;                                   // I-Anteil
     if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
     if(SummeNick >  16000) SummeNick =  16000;
     if(SummeNick < -16000) SummeNick = -16000;
     pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
     // Motor Vorn
         Motor_Hinten = motorwert;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
+    if(IntegralRoll) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;
-        SummeRoll += DiffRoll;                                                              // I-Anteil
+    else                 SummeRoll += DiffRoll;                                                             // I-Anteil
     if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
     if(SummeRoll >  16000) SummeRoll =  16000;
     if(SummeRoll < -16000) SummeRoll = -16000;
     pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
     if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));

Subversion Repositories FlightCtrl

(root)/tags/V0.69k/fc.c @ 1904 - Rev 419 → 469