WebSVN - FlightCtrl - Diff - Rev 1654 and 1658 - /branches/V0.82a_ACC-HH

 int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;
 unsigned int NeutralAccX=0, NeutralAccY=0;
 int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
 int NeutralAccZ = 0;
-unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
+unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0 , ControlHeading = 0;
 long IntegralNick = 0,IntegralNick2 = 0;
 long IntegralRoll = 0,IntegralRoll2 = 0;
 long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
 long Integral_Gier = 0;
 long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
 unsigned char Parameter_NaviOperatingRadius;
 unsigned char Parameter_NaviWindCorrection;
 unsigned char Parameter_NaviSpeedCompensation;
 unsigned char Parameter_ExternalControl;
 unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
+unsigned char HeadFree = 0;
 signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 int MaxStickNick = 0,MaxStickRoll = 0;
 unsigned int  modell_fliegt = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 void CopyDebugValues(void)
+{
     DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
     DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
     DebugOut.Analog[2] = Mittelwert_AccNick / 4;
     DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);//AdWertAccHoch;// Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;
     DebugOut.Analog[8] = KompassValue;
     DebugOut.Analog[9] = UBat;
     DebugOut.Analog[10] = SenderOkay;
     DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
-DebugOut.Analog[16] = NeutralAccZ;
+//DebugOut.Analog[16] = NeutralAccZ;
         //DebugOut.Analog[16] = Motor[0].Temperature;
     //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
 //    DebugOut.Analog[18] = VarioMeter;
 //    DebugOut.Analog[19] = WinkelOut.CalcState;
     DebugOut.Analog[20] = ServoNickValue;
     Mess_Integral_Gier = 0;
     StartLuftdruck = Luftdruck;
     VarioMeter = 0;
     Mess_Integral_Hoch = 0;
     KompassStartwert = KompassValue;
+ControlHeading = KompassValue / 15;
     GPS_Neutral();
     beeptime = 50;
         Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
         Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
     ExternHoehenValue = 0;
             tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
             tmpl *= Parameter_AchsKopplung1;  // 90
             tmpl /= 4096L;
             tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
             tmpl2 *= Parameter_AchsKopplung1;
             tmpl2 /= 4096L;
             if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
             //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
+         }
       else  tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;
-TrimRoll = tmpl - tmpl2 / 100L;
+                        TrimRoll = tmpl - tmpl2 / 100L;
-TrimNick = -tmpl2 + tmpl / 100L;
+                        TrimNick = -tmpl2 + tmpl / 100L;
 // Kompasswert begrenzen  ++++++++++++++++++++++++++++++++++++++++++++++++
                     if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag
                     if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
 // Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
             Mess_IntegralRoll2 += MesswertRoll + TrimRoll;
 // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
  CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
  CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
  Ki = (10300 / 2) / (Parameter_I_Faktor + 1);
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
+if(Poti4 > 50) HeadFree = 1; else HeadFree = 0;
+ if(HeadFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;}
+}
 //############################################################################
 //
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG))
+  {
+        static int stick_nick,stick_roll;
+unsigned char angle = 180/15;
+signed char sintab[31] = { 0, 4, 8, 11, 14, 16, 16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16, 16};
+//signed char costab[24] = {16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16};
+angle = (ControlHeading + (360/15) - ErsatzKompass / (GIER_GRAD_FAKTOR * 15)) % 24;
+signed int cos_h, sin_h;
+cos_h = sintab[angle + 6]/2;
+sin_h = sintab[angle]/2;
         static int stick_nick,stick_roll;
     ParameterZuordnung();
     stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
-    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
-    StickNick = stick_nick - (GPS_Nick + GPS_Nick2);
     stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
     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;
+    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
+if(HeadFree)
+ {
+    StickNick = ((cos_h * stick_nick) + (sin_h * stick_roll))/8;
+    StickRoll = ((cos_h * stick_roll) - (sin_h * stick_nick))/8;
+ }
+ else
+ {
+    StickNick = stick_nick;
+    StickRoll = stick_roll;
     StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
+        if(StickGier > 2) StickGier -= 2;       else
+        if(StickGier < -2) StickGier += 2; else StickGier = 0;
         if(StickGier > 2) StickGier -= 2;       else
-        if(StickGier < -2) StickGier += 2; else StickGier = 0;
+    StickNick -= (GPS_Nick + GPS_Nick2);
     StickRoll -= (GPS_Roll + GPS_Roll2);
         StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
     if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//DebugOut.Analog[16] = KompassSignalSchlecht;
+DebugOut.Analog[18] = KompassSignalSchlecht;
     if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+     {
        int w,v,r,fehler,korrektur;
        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 / 8 + 1;
+       korrektur = w / 8 + 2;
        fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
        if(abs(MesswertGier) > 128)
+            {
-                 fehler = 0;
+//               korrektur = korrektur = w / 16 + 2; // schnell konvergieren
+                }
        if(!KompassSignalSchlecht && w < 25)
+        {
         GierGyroFehler += fehler;
-        if(NeueKompassRichtungMerken)
-         {
-//         beeptime = 200;
+        if(NeueKompassRichtungMerken)
 //         KompassStartwert = KompassValue;
                   ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
           KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
           NeueKompassRichtungMerken = 0;
+         }
+        }
-         }
+DebugOut.Analog[16] = fehler;
 DebugOut.Analog[17] = korrektur;
-       ErsatzKompass += (fehler * 8) / korrektur;
+       ErsatzKompass += (fehler * 16) / korrektur;
        w = (w * Parameter_KompassWirkung) / 32;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
        if(w >= 0)

Subversion Repositories FlightCtrl

(root)/branches/V0.82a_ACC-HH_MartinR/fc.c @ 1815 - Rev 1654 → 1658