WebSVN - FlightCtrl - Diff - Rev 55 and 173 - /branches/V0.70d_X

 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include "main.h"
 unsigned char h,m,s;
-volatile unsigned char Timeout = 0;
+volatile unsigned int I2CTimeout = 100;
 volatile int MesswertNick,MesswertRoll,MesswertGier;
 volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
 volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 volatile unsigned char SenderOkay = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0;
 char MotorenEin = 0;
 int HoehenWert = 0;
 int SollHoehe = 0;
+unsigned char Looping_Nick = 0,Looping_Roll = 0;
 float Kp =  FAKTOR_P;
 float Ki =  FAKTOR_I;
 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_P = 50;            // Wert : 10-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_UserParam2 = 0;
 unsigned char Parameter_UserParam3 = 0;
+unsigned char Parameter_UserParam4 = 0;
-unsigned char Parameter_UserParam4 = 0;
+unsigned char Parameter_ServoNickControl = 100;
-unsigned char Parameter_ServoNickControl = 100;
+unsigned char Parameter_LoopGasLimit = 70;
 struct mk_param_struct EE_Parameter;
         CalibrierMittelwert();
     if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+     {
       if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+     }
+    if(PlatinenVersion == 10)
+    {
-    AdNeutralNick= abs(MesswertNick);
+     AdNeutralNick= abs(MesswertNick);
-        AdNeutralRoll= abs(MesswertRoll);
+         AdNeutralRoll= abs(MesswertRoll);
-        AdNeutralGier= abs(MesswertGier);
+         AdNeutralGier= abs(MesswertGier);
+    }
+    else
+    {
+     AdNeutralNick= abs(MesswertNick) / 2;
+         AdNeutralRoll= abs(MesswertRoll) / 2;
+         AdNeutralGier= abs(MesswertGier) / 2;
+    }
     NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
         NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
         NeutralAccZ = Aktuell_az;
     IntegralNick2 = Mess_IntegralNick2;
     IntegralRoll2 = Mess_IntegralRoll2;
     // ADC einschalten
     ANALOG_ON;
 /*
 //------------------------------------------------------------------------------
     if(MesswertNick > 200)  MesswertNick += 4 * (MesswertNick - 200);
     else
-    if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
     if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
     if(MesswertRoll > 200)  MesswertRoll += 4 * (MesswertRoll - 200);
     else
     if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
+//------------------------------------------------------------------------------
-//------------------------------------------------------------------------------
+*/
     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--;
  EE_Parameter.Kanalbelegung[K_POTI2] = 6;
  EE_Parameter.Kanalbelegung[K_POTI3] = 7;
  EE_Parameter.Kanalbelegung[K_POTI4] = 8;
  EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;
  EE_Parameter.Hoehe_MinGas = 30;
- EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
+ EE_Parameter.MaxHoehe     = 251;      // Wert : 0-32   251 -> Poti1
- EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
+ EE_Parameter.Hoehe_P      = 10;       // Wert : 0-32
- EE_Parameter.Luftdruck_D  = 70;      // Wert : 0-250
+ EE_Parameter.Luftdruck_D  = 50;       // Wert : 0-250
- EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250
+ EE_Parameter.Hoehe_ACC_Wirkung = 50;  // Wert : 0-250
- EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
+ EE_Parameter.Hoehe_Verstaerkung = 4;  // Wert : 0-50
- EE_Parameter.Stick_P = 4; //2           // Wert : 1-6
+ EE_Parameter.Stick_P = 4; //2         // Wert : 1-6
- EE_Parameter.Stick_D = 8; //8           // Wert : 0-64
+ EE_Parameter.Stick_D = 8; //8         // Wert : 0-64
- EE_Parameter.Gier_P = 16;            // Wert : 1-20
+ EE_Parameter.Gier_P = 16;             // Wert : 1-20
  EE_Parameter.Gas_Min = 15;            // Wert : 0-32
- EE_Parameter.Gas_Max = 250;          // Wert : 33-250
+ EE_Parameter.Gas_Max = 250;           // Wert : 33-250
- EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
+ EE_Parameter.GyroAccFaktor = 26;      // Wert : 1-64
  EE_Parameter.KompassWirkung = 128;    // Wert : 0-250
  EE_Parameter.Gyro_P = 120; //80          // Wert : 0-250
  EE_Parameter.Gyro_I = 150;               // Wert : 0-250
- EE_Parameter.UnterspannungsWarnung = 90; // Wert : 0-250
+ EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250
  EE_Parameter.NotGas = 35;                // Wert : 0-250     // Gaswert bei Empangsverlust
  EE_Parameter.NotGasZeit = 20;            // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
  EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
- EE_Parameter.I_Faktor = 5;
+ EE_Parameter.I_Faktor = 0;
  EE_Parameter.UserParam1 = 0;             //zur freien Verwendung
  EE_Parameter.UserParam2 = 0;             //zur freien Verwendung
  EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
  EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
  EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
  EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
  EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
- EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
+ EE_Parameter.ServoNickMin = 50;          // Wert : 0-250     // Anschlag
  EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
  EE_Parameter.ServoNickRefresh = 5;
+ EE_Parameter.LoopGasLimit = 50;
+ EE_Parameter.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
+ EE_Parameter.LoopConfig = 0;             // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt
- memcpy(EE_Parameter.Name, "Normal\0", 12);
+ memcpy(EE_Parameter.Name, "Sport\0", 12);
+}
 void DefaultKonstanten2(void)
+{
  EE_Parameter.Gas_Max = 250;          // Wert : 33-250
  EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
  EE_Parameter.KompassWirkung = 128;    // Wert : 0-250
  EE_Parameter.Gyro_P = 175; //80           // Wert : 0-250
  EE_Parameter.Gyro_I = 175;           // Wert : 0-250
- EE_Parameter.UnterspannungsWarnung = 90;  // Wert : 0-250
+ EE_Parameter.UnterspannungsWarnung = 94;  // Wert : 0-250
  EE_Parameter.NotGas = 35;                 // Wert : 0-250     // Gaswert bei Empangsverlust
  EE_Parameter.NotGasZeit = 20;             // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
  EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
- EE_Parameter.I_Faktor = 5;
+ EE_Parameter.I_Faktor = 0;
  EE_Parameter.UserParam1 = 0;   //zur freien Verwendung
  EE_Parameter.UserParam2 = 0;   //zur freien Verwendung
  EE_Parameter.UserParam3 = 0;   //zur freien Verwendung
  EE_Parameter.UserParam4 = 0;   //zur freien Verwendung
  EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
  EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
  EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
  EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
  EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
  EE_Parameter.ServoNickRefresh = 5;
+ EE_Parameter.LoopGasLimit = 50;
+ EE_Parameter.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
+ EE_Parameter.LoopConfig = 0;              // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
  memcpy(EE_Parameter.Name, "Kamera\0", 12);
+}
  CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
  CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
  CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
  CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
  CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
-   unsigned char ServoNickComp;          // Wert : 0-250     // Einfluss Gyro/Servo
-   unsigned char ServoNickCompInvert;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
-   unsigned char ServoNickMin;           // Wert : 0-250     // Anschlag
-   unsigned char ServoNickMax;           // Wert : 0-250     // Anschlag
  CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
  CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
  CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
+ CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
  Ki = (float) Parameter_I_Faktor * 0.0001;
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
+{
          int motorwert,pd_ergebnis,h,tmp_int;
          int GierMischanteil,GasMischanteil;
      static long SummeNick=0,SummeRoll=0;
      static long sollGier = 0,tmp_long,tmp_long2;
-     static int IntegralFehlerNick = 0;
+     long IntegralFehlerNick = 0;
-     static int IntegralFehlerRoll = 0;
+     long IntegralFehlerRoll = 0;
          static unsigned int RcLostTimer;
          static unsigned char delay_neutral = 0;
          static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
          static unsigned int  modell_fliegt = 0;
      static int hoehenregler = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
     if(GasMischanteil < 0) GasMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Emfang schlecht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
+        {
+        if(!PcZugriff)
+         {
+           if(BeepMuster == 0xffff)
+            {
-        if(!PcZugriff)  beeptime = 500;
+             beeptime = 15000;
+             BeepMuster = 0x0c00;
+            }
+         }
         if(RcLostTimer) RcLostTimer--;
         else
+         {
           MotorenEin = 0;
           Notlandung = 0;
             GasMischanteil = EE_Parameter.NotGas;
             Notlandung = 1;
             PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
             PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
             PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
-/*          Poti1 = 65;
-            Poti2 = 48;
-            Poti3 = 0;
-*/          }
+            }
          else MotorenEin = 0;
+        }
         else
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Emfang gut
+                    {
                     unsigned char setting;
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                         GRN_OFF;
-                        SetNeutral();
                         MotorenEin = 0;
                         delay_neutral = 0;
                         modell_fliegt = 0;
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
+                        {
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
                          eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
+                        }
-                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
-                        Piep(GetActiveParamSetNumber());
                         if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                          {
                              if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+                          }
+                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
+                        SetNeutral();
+                        Piep(GetActiveParamSetNumber());
+                        }
+                    }
                  else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
     StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
     StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
     StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
     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) ||
+     ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
+   {
+    Looping_Roll = 1;
+    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
+   }
+   else Looping_Roll = 0;
+  if(((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) ||
+     ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN))
+   {
+    Looping_Nick = 1;
+    Looping_Roll = 0;
+    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
+   }
+   else Looping_Nick = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Bei Empfangsausfall im Flug
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(Notlandung)
+    {
      StickGier = 0;
      StickNick = 0;
      StickRoll = 0;
      GyroFaktor  = 0.1;
+     IntegralFaktor = 0.005;
+     Looping_Roll = 0;
-     IntegralFaktor = 0.005;
+     Looping_Nick = 0;
+    }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gyro-Drift kompensieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         ZaehlMessungen = 0;
         if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
         if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
         if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
         if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
-        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;
+//        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)
-        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;
+//        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;   // macht nur mit Referenz (Kompass Sinn)
     ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
         Mess_IntegralNick2 = IntegralNick;
         Mess_IntegralRoll2 = IntegralRoll;
         Mess_Integral_Gier2 = Integral_Gier;
     ANALOG_ON;  // ADC einschalten
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Integrale auf ACC-Signal abgleichen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  if(IntegralFaktor && !Looping_Nick && !Looping_Roll)
+  {
-    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16;
+    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
+    if(labs(Mittelwert_AccNick) < 200) tmp_long /= 8;
+    else tmp_long /= 16;
-    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16;
+    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
+    if(labs(Mittelwert_AccRoll) < 200) tmp_long2 /= 8;
+    else tmp_long2 /= 16;
-#define AUSGLEICH 500
+ #define AUSGLEICH 500
     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;
+  }
+  else
+  {
+   tmp_long = 0;
+   tmp_long2 = 0;
+  }
  ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
     Mess_IntegralNick -= tmp_long;
     Mess_IntegralRoll -= tmp_long2;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    sollGier = StickGier;
     if(abs(StickGier) > 35)
+     {
       if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
+     }
-    tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo
+    tmp_int  = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo  y = ax + bx²
+    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
+    sollGier = tmp_int;
     Mess_Integral_Gier -= tmp_int;
-    if(Mess_Integral_Gier > 30000) Mess_Integral_Gier = 30000;  // begrenzen
+    if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000;  // begrenzen
-    if(Mess_Integral_Gier <-30000) Mess_Integral_Gier =-30000;
+    if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000;
  ANALOG_ON;     // ADC einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//  Kompass
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+//  Kompass
- //KompassValue = 12;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+     {
        int w,v;
           if(SignalSchlecht) SignalSchlecht--;
+        }
         else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
+     }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Debugwerte zuordnen
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-DebugOut.Sekunden++;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!TimerWerteausgabe--)
+   {
-    TimerWerteausgabe = 49;
-//    DebugOut.Analog[0] = MesswertNick;
-//    DebugOut.Analog[1] = MesswertRoll;
-//    DebugOut.Analog[2] = MesswertGier;
+    TimerWerteausgabe = 49;
     DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
     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[7] = GasMischanteil;
+    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
+    DebugOut.Analog[8] = KompassValue;
+/*    DebugOut.Analog[16] = motor_rx[0];
+    DebugOut.Analog[17] = motor_rx[1];
+    DebugOut.Analog[18] = motor_rx[2];
+    DebugOut.Analog[19] = motor_rx[3];
+    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
+    DebugOut.Analog[20] /= 14;
+    DebugOut.Analog[21] = motor_rx[4];
+    DebugOut.Analog[22] = motor_rx[5];
+    DebugOut.Analog[23] = motor_rx[6];
+    DebugOut.Analog[24] = motor_rx[7];
+    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
+*/
-    DebugOut.Analog[8] = KompassValue;
+//    DebugOut.Analog[9] = MesswertNick;
 //    DebugOut.Analog[9] = SollHoehe;
 //    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 //    DebugOut.Analog[11] = KompassStartwert;
 //    DebugOut.Analog[10] = Parameter_Gyro_I;
 //    DebugOut.Analog[10] = GasMischanteil;
 //    DebugOut.Analog[3] = HoeheD * 32;
 //    DebugOut.Analog[4] = hoehenregler;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
+    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
-    MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
+    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
-    MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
+    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
+//    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
-    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
+    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
     // Maximalwerte abfangen
     #define MAX_SENSOR  2048
+     }
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  DebugOut.Analog[7] = GasMischanteil;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier-Anteil
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+#define MUL_G  1.4
+    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
-        GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
+//GierMischanteil = 0;
+    if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
+    if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
-    if(GierMischanteil > 100)  GierMischanteil = 100;
-    if(GierMischanteil < -100) GierMischanteil = -100;
+    if(GasMischanteil < 10) GierMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
     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
+    pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick
+    // Motor Vorn
+#define MUL  2
+    if((pd_ergebnis >  MUL * (GasMischanteil))) pd_ergebnis =  MUL * (GasMischanteil);
+    if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);
     // Motor Vorn
     motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
-        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
+        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;
-        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
+        if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
-        Motor_Hinten = motorwert;
         Motor_Hinten = motorwert;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
         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))) pd_ergebnis =  MUL * (GasMischanteil);
-    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
+    if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);
     // Motor Links
     motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
-        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
+        if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor_Links = motorwert;
     // Motor Rechts
         motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
-        if ((motorwert < 0) | (GasMischanteil < 10))    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;
-   // +++++++++++++++++++++++++++++++++++++++++++++++
    // +++++++++++++++++++++++++++++++++++++++++++++++

Subversion Repositories FlightCtrl

(root)/branches/V0.70d_X_Formation/fc.c @ 1157 - Rev 55 → 173