WebSVN - FlightCtrl - Diff - Rev 604 and 614 - /branches/V2.01a_ACC-HH

 unsigned char Parameter_AchsKopplung1 = 0;
 unsigned char Parameter_AchsGegenKopplung1 = 0;
 unsigned char Parameter_DynamicStability = 100;
 struct mk_param_struct EE_Parameter;
 signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
+int MaxStickNick = 0,MaxStickRoll = 0;
 void Piep(unsigned char Anzahl)
+{
  while(Anzahl--)
  if(!NewPpmData-- || Notlandung)
+  {
     int tmp_int;
         static int stick_nick,stick_roll;
     ParameterZuordnung();
-    StickNick = (StickNick * 7 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 8;
+    StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
     StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
-    StickRoll = (StickRoll * 7 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 8;
+    StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
     StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
+        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
+   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick)
+     MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--;
+   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll)
+     MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;
-        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
+   if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}
     GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
     IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
+    }
     if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
     if(GyroFaktor < 0) GyroFaktor = 0;
-    if(IntegralFaktor < 0) IntegralFaktor = 0;
-/*
-    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
-    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
-     {
-      if(IntegralNick >  60000)
-      {
-       StickNick -=  8 * EE_Parameter.Stick_P;
-       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;
-      }
-      else
-      if(IntegralNick < -60000)
-      {
-       StickNick += 8 * EE_Parameter.Stick_P;
-       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;
-      }
-      if(IntegralRoll >  60000)
-      {
-       StickRoll -=  8 * EE_Parameter.Stick_P;
-       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;
-      }
-      else
-      if(IntegralRoll < -60000)
-      {
-       StickRoll += 8 * EE_Parameter.Stick_P;
-       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;
-      }
-     }
-*/
+    if(IntegralFaktor < 0) IntegralFaktor = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!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_long /= 16;
     tmp_long2 /= 16;
+   if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))
+    {
+    tmp_long  /= 3;
+    tmp_long2 /= 3;
+    }
- #define AUSGLEICH 32 //(Parameter_UserParam1 / 2)
+ #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;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
 #define MAX_I 0//(Poti2/10)
 // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
     ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
-    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
 // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
     ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
+    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
     LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
+   if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))
+    {
-//    Mess_IntegralNick -= ausgleichNick;
+     LageKorrekturNick /= 2;
+     LageKorrekturNick /= 2;
 //    Mess_IntegralRoll -= ausgleichRoll;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gyro-Drift ermitteln
          } else  last_r_n = 0;
         } else
+        {
          cnt = 0;
+        }
-DebugOut.Analog[27] = ausgleichRoll;
         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 //if(cnt > 1) beeptime = 50;
         if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
         if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
+DebugOut.Analog[27] = ausgleichRoll;
 DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
 DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
+  }
   else
+  {
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = MesswertNick - (StickNick - GPS_Nick);   // Differenz bestimmen
     if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
     else  SummeNick += DiffNick; // I-Anteil bei HH
-    if(SummeNick > 0) SummeNick-= 2 ; else SummeNick += 2 ;
+//    if(SummeNick > 0) SummeNick-= 2 ; else SummeNick += 2 ;
     if(SummeNick >  16000) SummeNick =  16000;
     if(SummeNick < -16000) SummeNick = -16000;
     pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
     // Motor Vorn
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = MesswertRoll - (StickRoll  - GPS_Roll);      // Differenz bestimmen
     if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll  - GPS_Roll);// I-Anteil bei Winkelregelung
     else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
-    if(SummeRoll > 0) SummeRoll-= 2 ; else SummeRoll += 2 ;
+//    if(SummeRoll > 0) SummeRoll-= 2 ; else SummeRoll += 2 ;
     if(SummeRoll >  16000) SummeRoll =  16000;
     if(SummeRoll < -16000) SummeRoll = -16000;
     pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;

Subversion Repositories FlightCtrl

(root)/branches/V2.01a_ACC-HH_MartinR/fc.c @ 2384 - Rev 604 → 614