WebSVN - FlightCtrl - Diff - Rev 1111 and 1120 - /branches/V0.82a_ACC-HH

 unsigned char Parameter_UserParam6 = 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_AchsKopplung1 = 90;
-unsigned char Parameter_AchsKopplung2 = 6;
+unsigned char Parameter_AchsKopplung2 = 65;
+unsigned char Parameter_CouplingYawCorrection = 64;
-unsigned char Parameter_AchsGegenKopplung1 = 0;
+//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
 unsigned char Parameter_J17Timing;              // 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;
+unsigned long GIER_GRAD_FAKTOR = 1291;
 void Piep(unsigned char Anzahl)
+{
  while(Anzahl--)
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
         AdNeutralGier = 0;
     AdNeutralGierBias = 0;
     Parameter_AchsKopplung1 = 0;
-    Parameter_AchsGegenKopplung1 = 0;
+    Parameter_AchsKopplung2 = 0;
     ExpandBaro = 0;
     CalibrierMittelwert();
     Delay_ms_Mess(100);
         CalibrierMittelwert();
     if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
         MesswertGier = (signed int) AdNeutralGier - AdWertGier;
     ErsatzKompass += MesswertGier;
 //    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
     MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
     MesswertNick = (signed int) AdWertNick - AdNeutralNick;
-DebugOut.Analog[21] = MesswertNick;
+//DebugOut.Analog[21] = MesswertNick;
-DebugOut.Analog[22] = MesswertRoll;
+//DebugOut.Analog[22] = MesswertRoll;
+DebugOut.Analog[22] = Mess_Integral_Gier;
 // 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  ++++++++++++++++++++++++++++++++++++++++++++++++
-//            Mess_Integral_Gier2 += MesswertGier;
+ Mess_Integral_Gier += MesswertGier;
 // Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
       if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
+         {
             tmpl3 = (MesswertRoll * Mess_IntegralNick) / 2048L;
-            tmpl3 *= Parameter_AchsKopplung1; //65
+            tmpl3 *= Parameter_AchsKopplung2; //65
             tmpl3 /= 4096L;
             tmpl4 = (MesswertNick * Mess_IntegralRoll) / 2048L;
-            tmpl4 *= Parameter_AchsKopplung1; //65
+            tmpl4 *= Parameter_AchsKopplung2; //65
             tmpl4 /= 4096L;
             tmpl4 -= tmpl3;
             ErsatzKompass += tmpl4;
             tmpl /= 4096L;
             tmpl2 = ((MesswertGier + tmpl4) * Mess_IntegralRoll) / 2048L;
             tmpl2 *= Parameter_AchsKopplung1;
             tmpl2 /= 4096L;
             if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
-            MesswertGier += tmpl4 / 2;
+            MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 128;
+         }
       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 += tmpl;
+            MesswertRoll += tmpl2 / 100L; //109
-            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
+//            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
             Mess_IntegralRoll2 += MesswertRoll;
+            Mess_IntegralRoll +=  MesswertRoll - LageKorrekturRoll;
             Mess_IntegralRoll +=  MesswertRoll - LageKorrekturRoll;
             if(Mess_IntegralRoll > Umschlag180Roll)
+            {
              Mess_IntegralRoll  = -(Umschlag180Roll - 25000L);
              Mess_IntegralRoll2 = Mess_IntegralRoll;
               if(AdWertRoll > 2000) MesswertRoll = +1000;
               if(AdWertRoll > 2015) MesswertRoll = +2000;
+                         }
 // Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
             MesswertNick -= tmpl2;
-            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
+//            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
+            MesswertNick -= tmpl / 100L; //109
             Mess_IntegralNick2 += MesswertNick;
             Mess_IntegralNick  += MesswertNick - LageKorrekturNick;
             if(Mess_IntegralNick > Umschlag180Nick)
     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
+#define D_LIMIT 8
   if(Parameter_Gyro_D)
   {
-   d2Nick = (((MesswertNick - oldNick) * (signed int) Parameter_Gyro_D));
+   d2Nick = (((MesswertNick - oldNick)));
    oldNick = MesswertNick;
    if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
    else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
-   MesswertNick += d2Nick;
+   MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D);
-   d2Roll = (((MesswertRoll - oldRoll) * (signed int) Parameter_Gyro_D));
+   d2Roll = (((MesswertRoll - oldRoll)));
    oldRoll = MesswertRoll;
    if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
    else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
-   MesswertRoll += d2Roll;
+   MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D);
+  }
  CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
  CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
  CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
  CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
  CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
+ CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255);
  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);
  CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
  CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
  CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
  CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
  CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
+ CHK_POTI(Parameter_AchsKopplung2,    EE_Parameter.AchsKopplung2,0,255);
+ CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255);
- CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
+// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
  CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
  CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
  CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
         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);
-//    StickNick  = (StickNick  * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
     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;
-    StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
     StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
+    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
-//    StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
+        if(StickGier > 2) StickGier -= 2;       else
         if(StickGier < -2) StickGier += 2; else StickGier = 0;
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
         StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!Looping_Nick && !Looping_Roll)
+  {
    long tmp_long, tmp_long2;
-   if(FromNaviCtrl_Value.Kalman_K != -1)
+   if(FromNaviCtrl_Value.Kalman_K != -1 && !TrichterFlug)
+     {
       tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
       tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
       tmp_long  = (tmp_long  * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
      if((MaxStickNick > 64) || (MaxStickRoll > 64))
+      {
       tmp_long  /= 3;
       tmp_long2 /= 3;
+      }
      if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
+      {
       tmp_long  /= 3;
       tmp_long2 /= 3;
+      }
  #define AUSGLEICH 32
         };
+     }
     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;
+DebugOut.Analog[21] = 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;

Subversion Repositories FlightCtrl

(root)/branches/V0.82a_ACC-HH_MartinR/fc.c @ 1797 - Rev 1111 → 1120