WebSVN - FlightCtrl - Diff - Rev 1839 and 1840

 long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
 long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
 long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
 long SummeNick=0,SummeRoll=0;
 volatile long Mess_Integral_Hoch = 0;
-int  KompassValue = 0;
+int  KompassValue = -1;
 int  KompassSollWert = 0;
 int  KompassRichtung = 0;
 char CalculateCompassTimer = 100;
 unsigned char KompassFusion = 32;
 unsigned int  KompassSignalSchlecht = 50;
 volatile unsigned char SenderOkay = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
 char MotorenEin = 0,StartTrigger = 0;
 long HoehenWert = 0;
 long SollHoehe = 0;
+int CompassGierSetpoint = 0;
 int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0;
 //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;
 //############################################################################
+{
     static signed long tmpl,tmpl2,tmpl3,tmpl4;
         static signed int oldNick, oldRoll, d2Roll, d2Nick;
         signed long winkel_nick, winkel_roll;
-    unsigned char i;
         MesswertGier = (signed int) AdNeutralGier - AdWertGier;
     MesswertNick = (signed int) AdWertNickFilter / 8;
     MesswertRoll = (signed int) AdWertRollFilter / 8;
     RohMesswertNick = MesswertNick;
     RohMesswertRoll = MesswertRoll;
+                {
                 SummeNick = 0;
                 SummeRoll = 0;
                 sollGier = 0;
                 Mess_Integral_Gier = 0;
-                NeueKompassRichtungMerken = 250;
                 } else FC_StatusFlags |= FC_STATUS_FLY;
             if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
+                {
                                                                         Mess_IntegralRoll2 = IntegralRoll;
                                                                         SummeNick = 0;
                                                                         SummeRoll = 0;
                                                                         FC_StatusFlags |= FC_STATUS_START;
                                                                         ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
+                                                                        NeueKompassRichtungMerken = 100; // 2 sekunden
+                                                                }
                                                                 else
+                                                                {
                                                                         beeptime = 1500; // indicate missing calibration
+                                                                }
     StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8;
     StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8;
+ }
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
-        if(StickGier > 2) StickGier -= 2;       else
+        if(StickGier > 4) StickGier -= 4;       else
-        if(StickGier < -2) StickGier += 2; else StickGier = 0;
+        if(StickGier < -4) StickGier += 4; else StickGier = 0;
     StickNick -= (GPS_Nick + GPS_Nick2);
  } //  ZaehlMessungen >= ABGLEICH_ANZAHL
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    if(abs(StickGier) > 15) // war 35
+    if(abs(StickGier) > 3) // war 15
+     {
 //      KompassSignalSchlecht = 1000;
       if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
        {
-         NeueKompassRichtungMerken = 250;
+         NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen
         };
+     }
     tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
+    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
-    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
+        tmp_int += CompassGierSetpoint;
     sollGier = 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;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+    if(KompassValue >= 0 && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+     {
       if(CalculateCompassTimer-- == 1)
+          {
-       int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt
-       CalculateCompassTimer = 13; // falls keine Navi-Daten
+       int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt
        CalculateCompassTimer = 13; // falls keine Navi-Daten
-// max. Korrekturwert schätzen
+           // max. Korrekturwert schätzen
        w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
        v = abs(IntegralRoll /512);
        if(v > w) w = v; // grösste Neigung ermitteln
        korrektur = w / 4 + 1;
-// Kompassfehlerwert bestimmen
+           // Kompassfehlerwert bestimmen
-       fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
+           fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
-// GIER_GRAD_FAKTOR ist ca. 1200
+           // GIER_GRAD_FAKTOR ist ca. 1200
+           // Kompasswert einloggen
+       if(KompassSignalSchlecht) KompassSignalSchlecht--;
-// Kompasswert einloggen
+           else
-       if(!KompassSignalSchlecht && w < 25)
+       if(w < 25)
+        {
         GierGyroFehler += fehler;
         if(NeueKompassRichtungMerken)
                     ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
             KompassSollWert = (ErsatzKompass/GIER_GRAD_FAKTOR);
+                   }
+         }
+        }
-// Kompass fusionieren
+       // Kompass fusionieren
        if(!KompassSignalSchlecht) ErsatzKompass += (fehler * KompassFusion) / korrektur;
-DebugOut.Analog[16] = fehler;
-DebugOut.Analog[17] = korrektur;
-DebugOut.Analog[18] = KompassFusion;
-DebugOut.Analog[19] = KompassSignalSchlecht;
-DebugOut.Analog[24] = FromNaviCtrl_Value.Kalman_K;
-// MK drehen
-    if(KompassSignalSchlecht) KompassSignalSchlecht--;
-    else
+     // MK Gieren
         if(!NeueKompassRichtungMerken)
        {
+           r = ((540 + (KompassSollWert - ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
-           r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassSollWert) % 360) - 180;
+DebugOut.Analog[19] = r;
+           v = r * (Parameter_KompassWirkung/2);  // nach Kompass ausrichten
+//           if(abs(r) > 20) v *= 2; // über 20° -> doppelt so schnell
+                   CompassGierSetpoint = v / 16;
-           v = r * (Parameter_KompassWirkung/2);  // nach Kompass ausrichten
+       }
+     else CompassGierSetpoint = 0;
-                   Mess_Integral_Gier += v;
+/*
-DebugOut.Analog[25] = v;
+DebugOut.Analog[16] = fehler;
-//DebugOut.Analog[26] = r;
+DebugOut.Analog[17] = korrektur;
+*/
+DebugOut.Analog[25] = CompassGierSetpoint;
 DebugOut.Analog[18] = KompassFusion;
 /*
        w = (w * Parameter_KompassWirkung) / 32;           // auf die Wirkung normieren
+        }
         else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
 */
       } // CalculateCompassTimer
+     }
+         else CompassGierSetpoint = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(TrichterFlug)  { SummeRoll = 0; SummeNick = 0;};

Subversion Repositories FlightCtrl

(root)/tags/V2.16a/fc.c @ 2684 - Rev 1839 → 1840