WebSVN - FlightCtrl - Diff - Rev 623 and 693 - /branches/V0.76g_ACC-HH

 // +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
 // +     for non-commercial use (directly or indirectly)
 // +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
 // +     with our written permission
 // +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
-// +     clearly linked as origin
-// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
+// +     clearly linked as origin // +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
 // +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 // +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 // +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 // +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 volatile int MesswertNick,MesswertRoll,MesswertGier;
 volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 volatile float NeutralAccZ = 0;
 unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
-volatile long IntegralNick = 0,IntegralNick2 = 0;
+long IntegralNick = 0,IntegralNick2 = 0;
-volatile long IntegralRoll = 0,IntegralRoll2 = 0;
+long IntegralRoll = 0,IntegralRoll2 = 0;
-volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
+long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
-volatile long Integral_Gier = 0;
+long Integral_Gier = 0;
-volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
+long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
-volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
+long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
-volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
+long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
-volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
+long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
 volatile long Mess_Integral_Hoch = 0;
 volatile int  KompassValue = 0;
 volatile int  KompassStartwert = 0;
 volatile int  KompassRichtung = 0;
+unsigned int  KompassSignalSchlecht = 500;
 unsigned char MAX_GAS,MIN_GAS;
 unsigned char Notlandung = 0;
 unsigned char HoehenReglerAktiv = 0;
 long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
+long  ErsatzKompass;
+int   ErsatzKompassInGrad; // Kompasswert in Grad
+int   GierGyroFehler = 0;
 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;
     GPS_Neutral();
     beeptime = 50;
         Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
         Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
     ExternHoehenValue = 0;
+    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
+    GierGyroFehler = 0;
+}
 //############################################################################
 // Bearbeitet die Messwerte
         Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
     IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
     IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
     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 = Mess_IntegralNick / 4096L;
             tmpl *= MesswertGier;
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = StickGas;
-    if(GasMischanteil < 0) GasMischanteil = 0;
+    if(GasMischanteil < MIN_GAS) GasMischanteil = MIN_GAS;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Emfang schlecht
+// Empfang schlecht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
+        {
         if(!PcZugriff)
+         {
+         {
           MotorenEin = 0;
           Notlandung = 0;
+         }
         ROT_ON;
-        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
+        if(modell_fliegt > 1000)  // wahrscheinlich in der Luft --> langsam absenken
+            {
             GasMischanteil = EE_Parameter.NotGas;
             Notlandung = 1;
             PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
             PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
             RcLostTimer = EE_Parameter.NotGasZeit * 50;
             if(GasMischanteil > 40)
+                {
                 if(modell_fliegt < 0xffff) modell_fliegt++;
+                }
-            if((modell_fliegt < 200) || (GasMischanteil < 40))
+            if((modell_fliegt < 200))// || (GasMischanteil < 40))
+                {
                 SummeNick = 0;
                 SummeRoll = 0;
+//                StickGier = 0;
-                Mess_Integral_Gier = 0;
+//                Mess_Integral_Gier = 0;
-                Mess_Integral_Gier2 = 0;
+//                Mess_Integral_Gier2 = 0;
+                }
             if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // auf Nullwerte kalibrieren
     Mess_IntegralNick2 -= IntegralFehlerNick;
     Mess_IntegralRoll2 -= IntegralFehlerRoll;
 //    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
+//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
+DebugOut.Analog[10] = GierGyroFehler;
+    if(GierGyroFehler > ABGLEICH_ANZAHL/2) AdNeutralGier++;
-//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
+    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) AdNeutralGier--;
     GierGyroFehler = 0;
 DebugOut.Analog[17] = IntegralAccNick / 26;
 DebugOut.Analog[18] = IntegralAccRoll / 26;
 DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
 //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+//    if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
     if(abs(StickGier) > 20) // war 35
      {
-      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
+      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) { NeueKompassRichtungMerken = 1; KompassSignalSchlecht = 500;};
+     }
     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;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+     {
-       int w,v;
+       int w,v,fehler,korrektur;
-       static int SignalSchlecht = 0;
+       static int KompassSignalSchlecht = 0;
        w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
        v = abs(IntegralRoll /512);
+//v /= 4;
+//w /= 4;
        if(v > w) w = v; // grösste Neigung ermitteln
+       korrektur = w + 3;
-       if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)
+       if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht)
+        {
          KompassStartwert = KompassValue;
          NeueKompassRichtungMerken = 0;
+        }
        w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
        if(w > 0)
+        {
+          if(!KompassSignalSchlecht)
+          {
+           fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
+           GierGyroFehler += fehler;
+//           DebugOut.Analog[10] = fehler;
+           ErsatzKompass += (fehler * 128) / korrektur;
-          if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
+           v = (KompassRichtung * w) / 32;  // nach Kompass ausrichten
+           w = Parameter_KompassWirkung;
+           if(v > w) v = w; // Begrenzen
+           else
+           if(v < -w) v = -w;
+           Mess_Integral_Gier += v;
+          }
-          if(SignalSchlecht) SignalSchlecht--;
+          if(KompassSignalSchlecht) KompassSignalSchlecht--;
+        }
-        else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
+        else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
+     }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DebugOut.Analog[3] = Mittelwert_AccRoll;
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[5] = HoehenWert;
     DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
     DebugOut.Analog[8] = KompassValue;
+    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[16] = motor_rx[0];
 // Gier-Anteil
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define MUL_G  1.0
     GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 // GierMischanteil = 0;
+#define MIN_GIERGAS  30  // unter diesem Gaswert trotzdem Gieren
+   if(GasMischanteil > MIN_GIERGAS)
+    {
-    if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
+     if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
-    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);
+    }
     if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil));
     if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS - GasMischanteil));
-    if(GasMischanteil < 20) GierMischanteil = 0;
+//    if(GasMischanteil < 20) GierMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = MesswertNick - (StickNick - GPS_Nick);   // Differenz bestimmen

Subversion Repositories FlightCtrl

(root)/branches/V0.76g_ACC-HH_MartinR/fc.c @ 1577 - Rev 623 → 693