WebSVN - FlightCtrl - Diff - Rev 1121 and 1153 - /branches/V0.88n_ACC-HH_HR

 #include "main.h"
 #include "eeprom.c"
 unsigned char h,m,s;
 volatile unsigned int I2CTimeout = 100;
-volatile int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias;
+int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
 int AdNeutralGierBias;
 int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
 long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
 long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
 long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
 long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
 volatile long Mess_Integral_Hoch = 0;
-volatile int  KompassValue = 0;
+int  KompassValue = 0;
-volatile int  KompassStartwert = 0;
+int  KompassStartwert = 0;
-volatile int  KompassRichtung = 0;
+int  KompassRichtung = 0;
 unsigned int  KompassSignalSchlecht = 500;
 unsigned char  MAX_GAS,MIN_GAS;
 unsigned char Notlandung = 0;
 unsigned char HoehenReglerAktiv = 0;
 unsigned char TrichterFlug = 0;
 long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 long  ErsatzKompass;
 int   ErsatzKompassInGrad; // Kompasswert in Grad
 int   GierGyroFehler = 0;
-float GyroFaktor;
+char GyroFaktor;
-float IntegralFaktor;
+char IntegralFaktor;
-volatile int  DiffNick,DiffRoll;
+int  DiffNick,DiffRoll;
 int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
 volatile unsigned char Motor1, Motor2,Motor3,Motor4,Motor5,Motor6,Motor7,Motor8;
 volatile unsigned char SenderOkay = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
 char MotorenEin = 0;
 int HoehenWert = 0;
 int SollHoehe = 0;
 int LageKorrekturRoll = 0,LageKorrekturNick = 0;
-float Ki =  FAKTOR_I;
+//float Ki =  FAKTOR_I;
+int Ki = 10300 / 33;
 unsigned char Looping_Nick = 0,Looping_Roll = 0;
 unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
 unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
 signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 int MaxStickNick = 0,MaxStickRoll = 0;
 unsigned int  modell_fliegt = 0;
 unsigned char MikroKopterFlags = 0;
 long GIER_GRAD_FAKTOR = 1291;
+signed int KopplungsteilNickRoll,KopplungsteilRollNick;
 void Piep(unsigned char Anzahl)
+{
  while(Anzahl--)
 void Mittelwert(void)
 //############################################################################
+{
     static signed long tmpl,tmpl2,tmpl3,tmpl4;
         static signed int oldNick, oldRoll, d2Roll, d2Nick;
         signed int tmp_int;
+        signed long winkel_nick, winkel_roll;
         MesswertGier = (signed int) AdNeutralGier - AdWertGier;
-    ErsatzKompass += MesswertGier;
 //    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
     MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
     MesswertNick = (signed int) AdWertNick - AdNeutralNick;
+    RohMesswertNick = MesswertNick;
+    RohMesswertRoll = MesswertRoll;
 //DebugOut.Analog[21] = MesswertNick;
 //DebugOut.Analog[22] = MesswertRoll;
 //DebugOut.Analog[22] = Mess_Integral_Gier;
     IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
     IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
     NaviAccNick    += AdWertAccNick;
     NaviAccRoll    += AdWertAccRoll;
     NaviCntAcc++;
-    IntegralAccZ    += Aktuell_az - NeutralAccZ;
+    IntegralAccZ  += Aktuell_az - NeutralAccZ;
+    if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;
+        else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;
+        else winkel_roll = Mess_IntegralRoll;
+    if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;
+        else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;
+        else winkel_nick = Mess_IntegralNick;
 // Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
- Mess_Integral_Gier += MesswertGier;
+   Mess_Integral_Gier += MesswertGier;
+   ErsatzKompass += MesswertGier;
 // Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
       if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
          {
-            tmpl3 = (MesswertRoll * Mess_IntegralNick) / 2048L;
+            tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
             tmpl3 *= Parameter_AchsKopplung2; //65
             tmpl3 /= 4096L;
-            tmpl4 = (MesswertNick * Mess_IntegralRoll) / 2048L;
+            tmpl4 = (MesswertNick * winkel_roll) / 2048L;
             tmpl4 *= Parameter_AchsKopplung2; //65
+            tmpl4 /= 4096L;
+            KopplungsteilNickRoll = tmpl3;
-            tmpl4 /= 4096L;
+            KopplungsteilRollNick = tmpl4;
             tmpl4 -= tmpl3;
             ErsatzKompass += tmpl4;
-            tmpl = ((MesswertGier + tmpl4) * Mess_IntegralNick) / 2048L;
+            tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
             tmpl *= Parameter_AchsKopplung1;  // 90
             tmpl /= 4096L;
-            tmpl2 = ((MesswertGier + tmpl4) * Mess_IntegralRoll) / 2048L;
+            tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
             tmpl2 *= Parameter_AchsKopplung1;
-            tmpl2 /= 4096L;
-            if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
+            tmpl2 /= 4096L;
+            if(labs(tmpl) > 128 || labs(tmpl2) > 128 /* || abs(KopplungsteilNickRoll) > 128 || abs(KopplungsteilRollNick) > 128)*/) TrichterFlug = 1;
+            //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
 DebugOut.Analog[21] = KopplungsteilNickRoll;
-            MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 128;
+DebugOut.Analog[22] = KopplungsteilRollNick;
-         }
       else  tmpl = tmpl2 = 0;
       else  tmpl = tmpl2 = 0;
 DebugOut.Analog[22] = tmpl;
 // Kompasswert begrenzen  ++++++++++++++++++++++++++++++++++++++++++++++++
                     if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag
     Integral_Gier  = Mess_Integral_Gier;
     IntegralNick = Mess_IntegralNick;
     IntegralRoll = Mess_IntegralRoll;
     IntegralNick2 = Mess_IntegralNick2;
     IntegralRoll2 = Mess_IntegralRoll2;
-  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
-  {
-    if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);
-    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
+    MesswertNick = (RohMesswertNick + 2 * MesswertNick) / 3;
-    if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);
-    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
+    MesswertRoll = (RohMesswertRoll + 2 * MesswertRoll) / 3;
-  }
 #define D_LIMIT 8
   if(Parameter_Gyro_D)
+  {
    if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
    else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
    MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D);
+  }
+ if(RohMesswertRoll > 0) MesswertRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
+ else                    MesswertRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
+ if(RohMesswertNick > 0) MesswertNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
+ else                    MesswertNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
+  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
+  {
+    if(RohMesswertNick > 256)       MesswertNick += 1 * (RohMesswertNick - 256);
+    else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);
+    if(RohMesswertRoll > 256)       MesswertRoll += 1 * (RohMesswertRoll - 256);
+    else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256);
+  }
     if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
     if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
     if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
     if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 // CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255);
 // CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255);
  CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);
  //Ki = (float) Parameter_I_Faktor * 0.0001;
- Ki = (float) Parameter_I_Faktor * 0.0001;
+ Ki = 10300 / (Parameter_I_Faktor + 1);
  MAX_GAS = EE_Parameter.Gas_Max;
 //############################################################################
 //
 void MotorRegler(void)
 //############################################################################
+{
-         int motorwert,pd_ergebnis,h,tmp_int;
+         int motorwert,pd_ergebnis, pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int;
          int GierMischanteil,GasMischanteil;
      static long SummeNick=0,SummeRoll=0;
      static long sollGier = 0,tmp_long,tmp_long2;
      static long IntegralFehlerNick = 0;
      static long IntegralFehlerRoll = 0;
          static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
      static int hoehenregler = 0;
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
      static long ausgleichNick, ausgleichRoll;
+     int IntegralNickMalFaktor,IntegralRollMalFaktor;
         Mittelwert();
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+            }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // neue Werte von der Funke
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!NewPpmData-- || Notlandung)
+  {
     int tmp_int;
         static int stick_nick,stick_roll;
     stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
     stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
-    StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
     StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
         if(StickGier > 2) StickGier -= 2;       else
 /*   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--;
 */
-    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / (256.0/STICK_GAIN);
+//    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / (256.0/STICK_GAIN);
+    GyroFaktor     = (Parameter_Gyro_P + 10.0);
-    IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
+//    IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
+    IntegralFaktor = Parameter_Gyro_I;
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Analoge Steuerung per Seriell
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
       if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
+     }
+   }
    if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
-   if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
+   if(Looping_Oben  || Looping_Unten) {  Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
   } // Ende neue Funken-Werte
   if(Looping_Roll || Looping_Nick)
+   {
     if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
-    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
+        TrichterFlug = 1;
+   }
    if(Notlandung)
+    {
      StickGier = 0;
      StickNick = 0;
      StickRoll = 0;
-     GyroFaktor     = (float) 100 / (256.0 / STICK_GAIN);
+     GyroFaktor     = 90;//(float) 100 / (256.0 / STICK_GAIN);
-     IntegralFaktor = (float) 120 / (44000 / STICK_GAIN);
+     IntegralFaktor = 120;//(float) 120 / (44000 / STICK_GAIN);
      Looping_Roll = 0;
      Looping_Nick = 0;
+    }
   if(EE_Parameter.Driftkomp)
+   {
     if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
     if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
+   }
-DebugOut.Analog[22] = MittelIntegralRoll / 26;
+//DebugOut.Analog[22] = MittelIntegralRoll / 26;
 //DebugOut.Analog[24] = GierGyroFehler;
     GierGyroFehler = 0;
         };
+     }
     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;
-DebugOut.Analog[21] = tmp_int;
     Mess_Integral_Gier -= tmp_int;
     if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
     if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  if(TrichterFlug)  { SummeRoll = 0; SummeNick = 0;};
+    if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;
+    if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0;
+    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN);
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
-    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
-    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
+    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN);
-    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 #ifdef OCTO
-    MesswertGier =   MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
+    MesswertGier =   (long)(MesswertGier * 4 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (4 * (44000 / STICK_GAIN));
 #else
-    MesswertGier =   MesswertGier * (4 * GyroFaktor) + Integral_Gier * IntegralFaktor / 4;
+    MesswertGier =   (long)(MesswertGier * 2 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (2 * (44000 / STICK_GAIN));
 #endif
+    // Maximalwerte abfangen
-    // Maximalwerte abfangen
+//    #define MAX_SENSOR  (4096*STICK_GAIN)
-    #define MAX_SENSOR  (4096*STICK_GAIN)
+    #define MAX_SENSOR  (4096*4)
     if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
     if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
     if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
       GasMischanteil = hoehenregler;
+     }
+  }
   if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
 #ifdef OCTO
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
-    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
+    if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
     else  SummeNick += DiffNick; // I-Anteil bei HH
     if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
     if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
-    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
+    pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick
     // Motor Vorn
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
-    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
+    if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
+    if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Roll-Achse
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
+    if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
+    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
+    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
+    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
+    pd_ergebnis_roll = DiffRoll + SummeRoll / Ki;       // PI-Regler für Roll
+    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
+    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
+    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
+// Octo-Mischer
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
+    motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil;
     motorwert /= STICK_GAIN;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor1 = motorwert;
-    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;   // Mischer
+    motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil;
     motorwert /= STICK_GAIN;
         if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
-        Motor2 = motorwert;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor2 = motorwert;
-    // Motor Heck
-        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
+        motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil;
     motorwert /= STICK_GAIN;
-        if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
+        if ((motorwert < 0)) motorwert = 0;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
-        Motor5 = motorwert;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    Motor3 = motorwert;
     // Motor Heck
-        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
+        motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil;
-    motorwert /= STICK_GAIN;
-        if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
-        Motor6 = motorwert;
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Roll-Achse
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
-    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung
-    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
-    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
-    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
-    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
+    motorwert /= STICK_GAIN;
-    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
+        if ((motorwert < 0)) motorwert = 0;
-    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
-    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
     Motor4 = motorwert;
-    // Motor Links
-    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
-    motorwert /= STICK_GAIN;
+        motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil;
-        if ((motorwert < 0)) motorwert = 0;
+    motorwert /= STICK_GAIN;
-        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        if ((motorwert < 0)) motorwert = 0;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
-    Motor8 = motorwert;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        Motor5 = motorwert;
-    // Motor Links
-    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;
-    motorwert /= STICK_GAIN;
+        motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil;
-        if ((motorwert < 0)) motorwert = 0;
+    motorwert /= STICK_GAIN;
-        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        if ((motorwert < 0)) motorwert = 0;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
-    Motor7 = motorwert;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
-    // Motor Rechts
+        Motor6 = motorwert;
         motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
-    motorwert /= STICK_GAIN;
+    motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil;
-        if ((motorwert < 0)) motorwert = 0;
+    motorwert /= STICK_GAIN;
-        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        if ((motorwert < 0)) motorwert = 0;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
-    Motor3 = motorwert;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    Motor7 = motorwert;
+    motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
-    // Motor Rechts
+    Motor8 = motorwert;
-        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
+   // +++++++++++++++++++++++++++++++++++++++++++++++
-    motorwert /= STICK_GAIN;
+/*
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
-    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
+    if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
     else  SummeNick += DiffNick; // I-Anteil bei HH
     if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
     if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
-    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
+    pd_ergebnis = DiffNick + SummeNick / Ki; // PI-Regler für Nick
     // Motor Vorn
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
         Motor_Hinten = motorwert;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
-    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung
+    if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
     else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
     if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
     if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
-    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
+    pd_ergebnis = DiffRoll + SummeRoll / Ki;    // 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;
     if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
     // Motor Links
     motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
     Motor_Rechts = motorwert;
    // +++++++++++++++++++++++++++++++++++++++++++++++
 //Motor_Hinten = 0;
 //Motor_Vorne = 0;
+//Motor_Links = 0;
+//Motor_Rechts = 0;
+}

Subversion Repositories FlightCtrl

(root)/branches/V0.88n_ACC-HH_HR_MartinR/fc.c @ 2177 - Rev 1121 → 1153