WebSVN - FlightCtrl - Diff - Rev 492 and 498 - /branches/V0.68d CRK HexaLotte/fc.c

 volatile unsigned char SenderOkay = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0;
 char MotorenEin = 0;
 int HoehenWert = 0;
 int SollHoehe = 0;
-int I_LageRoll = 0,I_LageNick = 0;
+int LageKorrekturRoll = 0,LageKorrekturNick = 0;
 float Kp =  FAKTOR_P;
 float Ki =  FAKTOR_I;
 unsigned char Looping_Nick = 0,Looping_Roll = 0;
 unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
       else  tmpl = tmpl2 = 0;
 // Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
             MesswertRoll += tmpl;
             MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
             Mess_IntegralRoll2 += MesswertRoll;
-            Mess_IntegralRoll +=  MesswertRoll - I_LageRoll;
+            Mess_IntegralRoll +=  MesswertRoll - LageKorrekturRoll;
             if(Mess_IntegralRoll > Umschlag180Roll)
+            {
              Mess_IntegralRoll  = -(Umschlag180Roll - 10000L);
              Mess_IntegralRoll2 = Mess_IntegralRoll;
+            }
+                         }
 // Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
             MesswertNick -= tmpl2;
             MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
             Mess_IntegralNick2 += MesswertNick;
-            Mess_IntegralNick  += MesswertNick - I_LageNick;
+            Mess_IntegralNick  += MesswertNick - LageKorrekturNick;
             if(Mess_IntegralNick > Umschlag180Nick)
+            {
              Mess_IntegralNick = -(Umschlag180Nick - 10000L);
              Mess_IntegralNick2 = Mess_IntegralNick;
+            }
     MittelIntegralNick2 = 0;
     MittelIntegralRoll2 = 0;
     Mess_IntegralNick2 = Mess_IntegralNick;
     Mess_IntegralRoll2 = Mess_IntegralRoll;
     ZaehlMessungen = 0;
-    I_LageNick = 0;
+    LageKorrekturNick = 0;
-    I_LageRoll = 0;
+    LageKorrekturRoll = 0;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  if(IntegralFaktor && !Looping_Nick && !Looping_Roll)
+  if(!Looping_Nick && !Looping_Roll)
+  {
    long tmp_long, tmp_long2;
     tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
     tmp_long /= 16;
     IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
 #define MAX_I 0//(Poti2/10)
 // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
     ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
-/*    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)
-     {
-       tmp_long = IntegralFehlerNick / 256L;
+    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
-       if(tmp_long > MAX_I) tmp_long = MAX_I;
-       if(tmp_long <-MAX_I) tmp_long =-MAX_I;
-       I_LageNick = tmp_long;
-     }
-     else
-     {
-      I_LageNick /= 2;
-     }
-*/
 // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
     ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
-/*    if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000)
-     {
-      tmp_long2 = IntegralFehlerRoll / 256L;
+    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
-      if(tmp_long2 > MAX_I) tmp_long2 = MAX_I;
-      if(tmp_long2 <-MAX_I) tmp_long2 =-MAX_I;
-      I_LageRoll = tmp_long2;
-     }
-     else
-     {
-      I_LageRoll /=2;
-     }
-*/
-    Mess_IntegralNick -= ausgleichNick;
+//    Mess_IntegralNick -= ausgleichNick;
-    Mess_IntegralRoll -= ausgleichRoll;
+//    Mess_IntegralRoll -= ausgleichRoll;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gyro-Drift ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
 DebugOut.Analog[21] = MittelIntegralNick / 26;
 DebugOut.Analog[22] = MittelIntegralRoll / 26;
 //DebugOut.Analog[28] = ausgleichNick;
 DebugOut.Analog[29] = ausgleichRoll;
-DebugOut.Analog[30] = I_LageRoll * 10;
+DebugOut.Analog[30] = LageKorrekturRoll * 10;
 #define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
 #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
 #define BEWEGUNGS_LIMIT 20000
            if(last_n_p)
+           {
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
             ausgleichNick = IntegralFehlerNick / 8;
             if(ausgleichNick > 5000) ausgleichNick = 5000;
+            LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
-            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;
              if(ausgleichNick < -5000) ausgleichNick = -5000;
+             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
-             Mess_IntegralNick -= ausgleichNick;
+//             Mess_IntegralNick -= ausgleichNick;
+            }
            else last_n_n = 1;
          } else  last_n_n = 0;
         } else cnt = 0;
         if(cnt > 4) cnt = 4;
         if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
 // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
         cnt = 1;// + labs(IntegralFehlerNick) / 4096;
-ausgleichRoll = 0;
+        ausgleichRoll = 0;
         if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
+        {
         if(IntegralFehlerRoll >  FEHLER_LIMIT2)
+         {
            if(last_r_p)
+           {
             cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
             ausgleichRoll = IntegralFehlerRoll / 8;
             if(ausgleichRoll > 5000) ausgleichRoll = 5000;
-            Mess_IntegralRoll -= ausgleichRoll;
+            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
-//            beeptime = 50;
+//            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 / 8;
             if(ausgleichRoll < -5000) ausgleichRoll = -5000;
-            Mess_IntegralRoll -= ausgleichRoll;
+//            Mess_IntegralRoll -= ausgleichRoll;
-//            beeptime = 50;
+            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
+           }
            else last_r_n = 1;
          } else  last_r_n = 0;
-        } else
         } else
          beeptime = 50;
          cnt = 0;
+        }
 DebugOut.Analog[27] = ausgleichRoll;
-        if(cnt > 4) cnt = 4;
+        if(cnt > 4) cnt = 4;
 //        if(cnt > Poti2 / 40) cnt =  Poti2 / 40;
 //if(cnt > 1) beeptime = 50;
         if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
         if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
-DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
-DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
+DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
+DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
+  }
+  else
+  {
+   LageKorrekturRoll = 0;
+   LageKorrekturNick = 0;
+  }
   if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++
    MittelIntegralNick_Alt = MittelIntegralNick;
    MittelIntegralRoll_Alt = MittelIntegralRoll;
     if(GasMischanteil < 20) GierMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
-    if(IntegralRoll) SummeNick += IntegralNick * IntegralFaktor - StickNick;
+    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
-    else  SummeNick += DiffNick;                                   // I-Anteil
+    else  SummeNick += DiffNick; // I-Anteil bei HH
     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;
+    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll  - GPS_Roll);// I-Anteil bei Winkelregelung
-    else                 SummeRoll += DiffRoll;                                                             // I-Anteil
+    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
     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)/branches/V0.68d CRK HexaLotte/fc.c @ 1204 - Rev 492 → 498