WebSVN - FlightCtrl - Diff - Rev 1167 and 1171 - /branches/V0.80g_ACC-HH_MartinR

 void SetNeutral(void)
 //############################################################################
+{
  unsigned char i;
  unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
+    ServoActive = 0; HEF4017R_ON;
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
      AdNeutralGierBias = AdNeutralGier;
      StartNeutralRoll = AdNeutralRoll;
      StartNeutralNick = AdNeutralNick;
     if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
+    {
-      NeutralAccY = abs(Mittelwert_AccRoll) / (4*ACC_AMPLIFY);
+      NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
-          NeutralAccX = abs(Mittelwert_AccNick) / (4*ACC_AMPLIFY);
+          NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
           NeutralAccZ = Aktuell_az;
+    }
     else
+    {
       NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
     LED_Init();
     MikroKopterFlags |= FLAG_CALIBRATE;
     FromNaviCtrl_Value.Kalman_K = -1;
     FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16;
     FromNaviCtrl_Value.Kalman_MaxFusion = 32;
+    if(Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110);
+    if(Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110);
+    if(Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110);
+    if(Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110);
+    ServoActive = 1;
+    SenderOkay = 100;
+}
 //############################################################################
 // Bearbeitet die Messwerte
 void Mittelwert(void)
 //############################################################################
+{
     static signed long tmpl,tmpl2,tmpl3,tmpl4;
-        static signed int oldNick, oldRoll, d2Roll, d2Nick;
-        signed int tmp_int;
+        static signed int oldNick, oldRoll, d2Roll, d2Nick;
+        signed long winkel_nick, winkel_roll;
         signed long winkel_nick, winkel_roll;
         MesswertGier = (signed int) AdNeutralGier - AdWertGier;
 //    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
-    MesswertNick = (signed int) AdWertNickFilter / 20;
+    MesswertNick = (signed int) AdWertNickFilter / 8;
-    MesswertRoll = (signed int) AdWertRollFilter / 20;
+    MesswertRoll = (signed int) AdWertRollFilter / 8;
     RohMesswertNick = MesswertNick;
     RohMesswertRoll = MesswertRoll;
 //DebugOut.Analog[21] = MesswertNick;
 //DebugOut.Analog[22] = MesswertRoll;
     NaviCntAcc++;
     IntegralAccZ  += Aktuell_az - NeutralAccZ;
 //++++++++++++++++++++++++++++++++++++++++++++++++
-// ADC einschalten
-        AdReady = 0;
+// ADC einschalten
+    ANALOG_ON;
-    ANALOG_ON;
+        AdReady = 0;
 //++++++++++++++++++++++++++++++++++++++++++++++++
     if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;
     IntegralNick2 = Mess_IntegralNick2;
     IntegralRoll2 = Mess_IntegralRoll2;
 #define D_LIMIT 128
-   MesswertNick = HiResNick / 20;
+   MesswertNick = HiResNick / 8;
-   MesswertRoll = HiResRoll / 20;
+   MesswertRoll = HiResRoll / 8;
    if(AdWertNick < 15)   MesswertNick = -1000;  if(AdWertNick <  7)   MesswertNick = -2000;
     LageKorrekturNick = 0;
     LageKorrekturRoll = 0;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  if(!Looping_Nick && !Looping_Roll)
+  if(!Looping_Nick && !Looping_Roll && Aktuell_az > 600)
+  {
    long tmp_long, tmp_long2;
-   if(FromNaviCtrl_Value.Kalman_K != -1 && !TrichterFlug)
+   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);
       tmp_long2 /= 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;
+      }
     if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
+DebugOut.Analog[16] = 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;
        fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
-           if(NeueKompassRichtungMerken)
+       if(abs(MesswertGier) > 128)
+            {
+                 fehler = 0;
+                }
+           if(NeueKompassRichtungMerken)
+            {
+//       ErsatzKompass += (fehler * 32) / korrektur;
-            {
+//               fehler = 0;
-                 fehler = 0;
+//               fehler /= 4;
-                 ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
+//               ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
+                }
        if(!KompassSignalSchlecht && w < 25)
+        {
         GierGyroFehler += fehler;
         if(NeueKompassRichtungMerken)
+         {
+          beeptime = 200;
-          beeptime = 200;
+//         KompassStartwert = KompassValue;
-//         KompassStartwert = KompassValue;
+                  ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
           KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
           NeueKompassRichtungMerken = 0;
+         }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!TimerWerteausgabe--)
+   {
     TimerWerteausgabe = 24;
-    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
+    DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
-    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
+    DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
-    DebugOut.Analog[2] = Mittelwert_AccNick;
+    DebugOut.Analog[2] = Mittelwert_AccNick / 4;
-    DebugOut.Analog[3] = Mittelwert_AccRoll;
+    DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[5] = HoehenWert;
-    DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);
+    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);//Aktuell_az;
     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[17] = FromNaviCtrl_Value.Distance;
     //DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
     DebugOut.Analog[19] = WinkelOut.CalcState;
     DebugOut.Analog[20] = ServoValue;
-    DebugOut.Analog[24] = MesswertNick/2;
+//    DebugOut.Analog[24] = MesswertNick/2;
-    DebugOut.Analog[25] = MesswertRoll/2;
+//    DebugOut.Analog[25] = MesswertRoll/2;
     DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
     DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
     DebugOut.Analog[30] = GPS_Nick;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(TrichterFlug)  { SummeRoll = 0; SummeNick = 0;};
-    if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;
+  if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;
-    if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0;
+  if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0;
 #define TRIM_MAX 200
       GasMischanteil = hoehenregler;
+     }
+  }
   if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   DebugOut.Analog[7] = GasMischanteil;
     motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil;
     motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
         Motor2 = motorwert;
-        motorwert = GasMischanteil + - pd_ergebnis_roll + GierMischanteil;
+        motorwert = GasMischanteil - pd_ergebnis_roll + GierMischanteil;
     motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
     Motor3 = motorwert;

Subversion Repositories FlightCtrl

(root)/branches/V0.80g_ACC-HH_MartinR_HR/fc.c @ 2222 - Rev 1167 → 1171