WebSVN - FlightCtrl - Diff - Rev 890 and 891 - /branches/MicroMag3

 long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
 long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
 long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
 long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
 volatile long Mess_Integral_Hoch = 0;
-volatile int  KompassValue = 0;
+int  KompassValue = 0;
-volatile int  KompassStartwert = 0;
+int  KompassStartwert = 0;
-volatile int  KompassRichtung = 0;
+int  KompassRichtung = 0;
+uint8_t updKompass;
 unsigned int  KompassSignalSchlecht = 500;
 unsigned char  MAX_GAS,MIN_GAS;
 unsigned char Notlandung = 0;
 unsigned char HoehenReglerAktiv = 0;
 unsigned char TrichterFlug = 0;
         acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
         acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
+        acc_neutral.Z = Aktuell_az;
+        Piep(2);
+        while (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100)       //Warten, bis Benutzer den Kopter neu ausgerichtet hat
+        Delay_ms_Mess(100);
-        acc_neutral.Z = Aktuell_az;
+        acc_neutral.compass = Aktuell_az;
         eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));
+}
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
-    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+        if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
-     {
+        {
+                int w,v,r,fehler,korrektur;
+                if (!updKompass--)              // Aufruf mit ~20 Hz
+                {
+                        updKompass = 25;
+                        KompassValue = heading_MM3();
+                        KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
-       int w,v,r,fehler,korrektur;
+                }
-       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
+                        w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
-       v = abs(IntegralRoll /512);
+                        v = abs(IntegralRoll /512);
-       if(v > w) w = v; // grösste Neigung ermitteln
+                        if(v > w) w = v; // grösste Neigung ermitteln
-       korrektur = w / 8 + 1;
+                        korrektur = w / 8 + 1;
-       if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht)
+                        if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht)
-        {
+                        {
-         beeptime = 200;
+                                beeptime = 200;
-//         KompassStartwert = KompassValue;
+                                //KompassStartwert = KompassValue;
-         KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
+                                KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
-         NeueKompassRichtungMerken = 0;
+                                NeueKompassRichtungMerken = 0;
-        }
+                        }
-       fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
+                        fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
-       ErsatzKompass += (fehler * 8) / korrektur;
+                        ErsatzKompass += (fehler * 8) / korrektur;
-       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
+                        w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
-       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
+                        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
-       if(w > 0)
+                        if(w > 0)
-        {
+                        {
-          if(!KompassSignalSchlecht)
+                                if(!KompassSignalSchlecht)
-          {
+                                {
-           GierGyroFehler += fehler;
+                                        GierGyroFehler += fehler;
-           v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
+                                        v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
-           r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
+                                        r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
-//           r = KompassRichtung;
+                                        //r = KompassRichtung;
-           v = (r * w) / v;  // nach Kompass ausrichten
+                                        v = (r * w) / v;  // nach Kompass ausrichten
-           w = 3 * Parameter_KompassWirkung;
+                                        w = 3 * Parameter_KompassWirkung;
-           if(v > w) v = w; // Begrenzen
-           else
+                                        if(v > w) v = w; // Begrenzen
-           if(v < -w) v = -w;
+                                        else if(v < -w) v = -w;
-           Mess_Integral_Gier += v;
+                                        Mess_Integral_Gier += v;
-          }
+                                }
-          if(KompassSignalSchlecht) KompassSignalSchlecht--;
+                        if(KompassSignalSchlecht) KompassSignalSchlecht--;
+                        }
+        else KompassSignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 0,5 sek
-        else KompassSignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 0,5 sek
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Subversion Repositories FlightCtrl

(root)/branches/MicroMag3_Nick666/V0.69k/fc.c @ 891 - Rev 890 → 891