WebSVN - FlightCtrl - Diff - Rev 1083 and 1111 - /branches/V0.80g_ACC-HH_MartinR

 float GyroFaktor;
 float IntegralFaktor;
 volatile 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;
 unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
 unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
 unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
 unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
 unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
+unsigned char Parameter_Gyro_D = 8;             // Wert : 0-250
 unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
 unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
 unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
 unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
 unsigned char Parameter_UserParam1 = 0;
 unsigned char Parameter_UserParam7 = 0;
 unsigned char Parameter_UserParam8 = 0;
 unsigned char Parameter_ServoNickControl = 100;
 unsigned char Parameter_LoopGasLimit = 70;
 unsigned char Parameter_AchsKopplung1 = 0;
+unsigned char Parameter_AchsKopplung2 = 6;
 unsigned char Parameter_AchsGegenKopplung1 = 0;
 unsigned char Parameter_DynamicStability = 100;
 unsigned char Parameter_J16Bitmask;             // for the J16 Output
 unsigned char Parameter_J16Timing;              // for the J16 Output
 unsigned char Parameter_J17Bitmask;             // for the J17 Output
 struct mk_param_struct EE_Parameter;
 signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 int MaxStickNick = 0,MaxStickRoll = 0;
 unsigned int  modell_fliegt = 0;
 unsigned char MikroKopterFlags = 0;
+unsigned int GIER_GRAD_FAKTOR = 1291;
 void Piep(unsigned char Anzahl)
+{
  while(Anzahl--)
 //############################################################################
 //  Nullwerte ermitteln
 void SetNeutral(void)
 //############################################################################
+{
+ unsigned char i;
+ unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
         CalibrierMittelwert();
     if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+     {
       if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+     }
+    for(i=0; i<32; i++)
+         {
+          Delay_ms_Mess(10);
+          gier_neutral += AdWertGier;
+          nick_neutral += MesswertNick;
+          roll_neutral += MesswertRoll;
+         }
-     AdNeutralNick= AdWertNick;
+     AdNeutralNick= nick_neutral / 32;
-         AdNeutralRoll= AdWertRoll;
+         AdNeutralRoll= roll_neutral / 32;
-         AdNeutralGier= AdWertGier;
+         AdNeutralGier= gier_neutral / 32;
-     AdNeutralGierBias = AdWertGier;
+     AdNeutralGierBias = AdNeutralGier;
      StartNeutralRoll = AdNeutralRoll;
      StartNeutralNick = AdNeutralNick;
     if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
+    {
       NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
     Mess_IntegralRoll2 = 0;
     Mess_Integral_Gier = 0;
     MesswertNick = 0;
     MesswertRoll = 0;
     MesswertGier = 0;
- Delay_ms_Mess(100);
+    Delay_ms_Mess(100);
     StartLuftdruck = Luftdruck;
     HoeheD = 0;
     Mess_Integral_Hoch = 0;
     KompassStartwert = KompassValue;
     GPS_Neutral();
 //############################################################################
 // Bearbeitet die Messwerte
 void Mittelwert(void)
 //############################################################################
+{
-    static signed long tmpl,tmpl2;
+    static signed long tmpl,tmpl2,tmpl3,tmpl4;
+        static signed int oldNick, oldRoll, d2Roll, d2Nick;
+        signed int tmp_int;
-    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
+        MesswertGier = (signed int) AdNeutralGier - AdWertGier;
+    ErsatzKompass += MesswertGier;
-    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
+//    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
     MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
     MesswertNick = (signed int) AdWertNick - AdNeutralNick;
-//DebugOut.Analog[26] = MesswertNick;
+DebugOut.Analog[21] = MesswertNick;
-DebugOut.Analog[28] = MesswertRoll;
+DebugOut.Analog[22] = MesswertRoll;
 // Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
         Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
         Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
     IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
     NaviAccNick    += AdWertAccNick;
     NaviAccRoll    += AdWertAccRoll;
     NaviCntAcc++;
     IntegralAccZ    += Aktuell_az - NeutralAccZ;
 // Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
-            ErsatzKompass +=  MesswertGier;
-            Mess_Integral_Gier +=  MesswertGier;
 //            Mess_Integral_Gier2 += MesswertGier;
-                    if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag
-                    if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
 // Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
       if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
+         {
-            tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L;
+            tmpl3 = (MesswertRoll * Mess_IntegralNick) / 2048L;
+            tmpl3 *= Parameter_AchsKopplung1; //65
+            tmpl3 /= 4096L;
+            tmpl4 = (MesswertNick * Mess_IntegralRoll) / 2048L;
+            tmpl4 *= Parameter_AchsKopplung1; //65
+            tmpl4 /= 4096L;
+            tmpl4 -= tmpl3;
+            ErsatzKompass += tmpl4;
+            tmpl = ((MesswertGier + tmpl4) * Mess_IntegralNick) / 2048L;
-            tmpl *= Parameter_AchsKopplung1;  //125
+            tmpl *= Parameter_AchsKopplung1;  // 90
             tmpl /= 4096L;
-            tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L;
+            tmpl2 = ((MesswertGier + tmpl4) * Mess_IntegralRoll) / 2048L;
             tmpl2 *= Parameter_AchsKopplung1;
             tmpl2 /= 4096L;
             if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
+            MesswertGier += tmpl4 / 2;
+         }
       else  tmpl = tmpl2 = 0;
+// Kompasswert begrenzen  ++++++++++++++++++++++++++++++++++++++++++++++++
+                    if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag
+                    if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
 // Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
             MesswertRoll += tmpl;
             MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
             Mess_IntegralRoll2 += MesswertRoll;
             Mess_IntegralRoll +=  MesswertRoll - LageKorrekturRoll;
               if(AdWertRoll > 1010) MesswertRoll = +1000;
               if(AdWertRoll > 1017) MesswertRoll = +2000;
+                         }
                          else
+                         {
-              if(AdWertRoll > 2020) MesswertRoll = +1000;
+              if(AdWertRoll > 2000) MesswertRoll = +1000;
-              if(AdWertRoll > 2034) MesswertRoll = +2000;
+              if(AdWertRoll > 2015) MesswertRoll = +2000;
+                         }
 // Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
             MesswertNick -= tmpl2;
             MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
             Mess_IntegralNick2 += MesswertNick;
               if(AdWertNick > 1010) MesswertNick = +1000;
               if(AdWertNick > 1017) MesswertNick = +2000;
+                         }
                          else
+                         {
-              if(AdWertNick > 2020) MesswertNick = +1000;
+              if(AdWertNick > 2000) MesswertNick = +1000;
-              if(AdWertNick > 2034) MesswertNick = +2000;
+              if(AdWertNick > 2015) MesswertNick = +2000;
+                         }
 //++++++++++++++++++++++++++++++++++++++++++++++++
 // ADC einschalten
     ANALOG_ON;
 //++++++++++++++++++++++++++++++++++++++++++++++++
     if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);
     else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
     if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);
     else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
+  }
+  if(PlatinenVersion >= 20) Parameter_Gyro_D = 8; else Parameter_Gyro_D = 0;
+#define D_LIMIT 64
+  if(Parameter_Gyro_D)
+  {
+   d2Nick = (((MesswertNick - oldNick) * (signed int) Parameter_Gyro_D));
+   oldNick = MesswertNick;
+   if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
+   else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
+   MesswertNick += d2Nick;
+   d2Roll = (((MesswertRoll - oldRoll) * (signed int) Parameter_Gyro_D));
+   oldRoll = MesswertRoll;
+   if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
+   else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
+   MesswertRoll += d2Roll;
+  }
     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--;
     if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 void SendMotorData(void)
 //############################################################################
+{
     if(!MotorenEin)
+        {
+#ifdef OCTO
+                Motor1 = 0;Motor2 = 0;Motor3 = 0;Motor4 = 0;Motor5 = 0;Motor6 = 0;Motor7 = 0;Motor8 = 0;
-        Motor_Hinten = 0;
+        if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];}
-        Motor_Vorne = 0;
+        if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];}
+        if(MotorTest[1]) {Motor5 = MotorTest[1]; Motor6 = MotorTest[1];}
-        Motor_Rechts = 0;
+        if(MotorTest[2]) {Motor7 = MotorTest[2]; Motor8 = MotorTest[2];}
+#else
-        Motor_Links = 0;
+        Motor_Hinten = 0; Motor_Vorne = 0; Motor_Rechts = 0; Motor_Links = 0;
         if(MotorTest[0]) Motor_Vorne = MotorTest[0];
         if(MotorTest[1]) Motor_Hinten = MotorTest[1];
         if(MotorTest[2]) Motor_Links = MotorTest[2];
         if(MotorTest[3]) Motor_Rechts = MotorTest[3];
+#endif
         MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
         } else MikroKopterFlags |= FLAG_MOTOR_RUN;
     DebugOut.Analog[12] = Motor_Vorne;
       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_IntegralNick -= tmp_long;
-    Mess_IntegralRoll -= tmp_long2;
+   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;
   static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
-  if(!Looping_Nick && !Looping_Roll && !TrichterFlug)
+  if(!Looping_Nick && !Looping_Roll && !TrichterFlug && (PlatinenVersion < 20))
+  {
     MittelIntegralNick  /= ABGLEICH_ANZAHL;
     MittelIntegralRoll  /= ABGLEICH_ANZAHL;
         IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
         IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
     Mess_IntegralNick2 -= IntegralFehlerNick;
     Mess_IntegralRoll2 -= IntegralFehlerRoll;
 //    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
+//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
+  if(EE_Parameter.Driftkomp)
 //    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
     if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
     if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
+   }
 DebugOut.Analog[22] = MittelIntegralRoll / 26;
 //DebugOut.Analog[24] = GierGyroFehler;
     DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
     DebugOut.Analog[2] = Mittelwert_AccNick;
     DebugOut.Analog[3] = Mittelwert_AccRoll;
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[5] = HoehenWert;
-    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
+    DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);
     DebugOut.Analog[8] = KompassValue;
     DebugOut.Analog[9] = UBat;
     DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
     DebugOut.Analog[10] = SenderOkay;
     //DebugOut.Analog[16] = Mittelwert_AccHoch;
     //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
     //DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
     DebugOut.Analog[19] = WinkelOut.CalcState;
     DebugOut.Analog[20] = ServoValue;
+    DebugOut.Analog[24] = MesswertNick/2;
+    DebugOut.Analog[25] = MesswertRoll/2;
     DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
     DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
     DebugOut.Analog[30] = GPS_Nick;
     DebugOut.Analog[31] = GPS_Roll;
     if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
     else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
     if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
+    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
-    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
+#ifdef OCTO
     MesswertGier =   MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
+#else
-    DebugOut.Analog[21] = MesswertNick;
+    MesswertGier =   MesswertGier * (4 * GyroFaktor) + Integral_Gier * IntegralFaktor / 4;
-    DebugOut.Analog[22] = MesswertRoll;
+#endif
     // Maximalwerte abfangen
     #define MAX_SENSOR  (4096*STICK_GAIN)
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Höhenregelung
 // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//OCR0B = 180 - (Poti1 + 120) / 4;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //DruckOffsetSetting = OCR0B;
-  GasMischanteil *= STICK_GAIN;
-/*
-  if(Poti1 > 200) {OCR0A = DruckOffsetSetting - 16; ExpandBaro = 16; }else
-  if(Poti1 > 150) {OCR0A = DruckOffsetSetting - 8; ExpandBaro = 8; }else
-  if(Poti1 > 100) {OCR0A = DruckOffsetSetting - 4; ExpandBaro = 4; }else
-  if(Poti1 >  50) {OCR0A = DruckOffsetSetting - 2; ExpandBaro = 1; }else
-  {OCR0A = DruckOffsetSetting; ExpandBaro = 0;}
-*/
+  GasMischanteil *= STICK_GAIN;
  if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
+  {
     int tmp_int;
         static char delay = 100;
       if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
       GasMischanteil = hoehenregler;
+     }
+  }
   if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
+#ifdef OCTO
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   DebugOut.Analog[7] = GasMischanteil;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
     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 /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        Motor2 = motorwert;
+    // Motor Heck
+        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        Motor5 = motorwert;
+    // Motor Heck
+        motorwert = GasMischanteil - pd_ergebnis - 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
+    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;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    Motor8 = motorwert;
+    // Motor Links
+    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    Motor7 = motorwert;
+    // Motor Rechts
+        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    Motor3 = motorwert;
+    // Motor Rechts
+        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    Motor4 = motorwert;
+   // +++++++++++++++++++++++++++++++++++++++++++++++
+}
+#else
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// + Mischer und PI-Regler
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  DebugOut.Analog[7] = GasMischanteil;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Gier-Anteil
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+#define MUL_G  1.0
+    GierMischanteil = MesswertGier - sollGier * STICK_GAIN;     // Regler für Gier
+//GierMischanteil = 0;
+#define MIN_GIERGAS  (40*STICK_GAIN)  // unter diesem Gaswert trotzdem Gieren
+   if(GasMischanteil > MIN_GIERGAS)
+    {
+     if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
+     if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
+    }
+    else
+    {
+     if(GierMischanteil > (MIN_GIERGAS / 2))  GierMischanteil = MIN_GIERGAS / 2;
+     if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
+    }
+    tmp_int = MAX_GAS*STICK_GAIN;
+    if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
+    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Nick-Achse
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
+    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - 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
+    // 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 < -tmp_int) pd_ergebnis = -tmp_int;
+    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor_Vorne = motorwert;
     // Motor Heck
         motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
     motorwert /= STICK_GAIN;
         if ((motorwert < 0)) motorwert = 0;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
     Motor_Rechts = motorwert;
    // +++++++++++++++++++++++++++++++++++++++++++++++
+//Motor_Hinten = 0;
+//Motor_Vorne = 0;
+}

Subversion Repositories FlightCtrl

(root)/branches/V0.80g_ACC-HH_MartinR_HR/fc.c @ 1993 - Rev 1083 → 1111