WebSVN - FlightCtrl - Diff - Rev 858 and 887 - /branches/MicroMag3

 //############################################################################
+{
         acc_neutral.X = 0;
         acc_neutral.Y = 0;
         acc_neutral.Z = 0;
+        Delay_ms(500);
+        beeptime = 100;
+        while (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100)       //Warten, bis Benutzer den Kopter neu ausgerichtet hat
     CalibrierMittelwert();
     Delay_ms_Mess(100);
         CalibrierMittelwert();
         acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
         acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
-        acc_neutral.Z = Aktuell_az;
-/*
-        while (PPM_in_gas > 100)        //Warten, bis Benutzer den Kopter neu ausgerichtet hat
-        {
-                uint8_t delay=9;
+        acc_neutral.Z = Aktuell_az;
-                Delay_ms(10);
-                if (!delay--)
-                {
+        beeptime = 100;
-                        delay = 9;
+        Delay_ms(500);
-                        beeptime = 100;
+        beeptime = 100;
-                }
+        while (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100)       //Warten, bis Benutzer den Kopter neu ausgerichtet hat
         Delay_ms_Mess(100);
-        acc_neutral.C = Aktuell_az;
+        acc_neutral.compass = Aktuell_az;
 */
         eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));
+}
     if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+     {
       if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+     }
-     AdNeutralNick= AdWertNick;
+     AdNeutralNick = AdWertNick;
-         AdNeutralRoll= AdWertRoll;
+         AdNeutralRoll = AdWertRoll;
-         AdNeutralGier= AdWertGier;
+         AdNeutralGier = AdWertGier;
      StartNeutralRoll = AdNeutralRoll;
      StartNeutralNick = AdNeutralNick;
 //############################################################################
 // Bearbeitet die Messwerte
 void Mittelwert(void)
 //############################################################################
+{
     static signed long tmpl,tmpl2;
+        uint8_t tmp_sreg = SREG;
+    cli();
-    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
+     MesswertGier = (int16_t)AdNeutralGier - AdWertGier;
-    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
+     MesswertRoll = (int16_t)AdWertRoll - AdNeutralRoll;
-    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
+     MesswertNick = (int16_t)AdWertNick - AdNeutralNick;
+         int16_t AdWertAccNick_temp = AdWertAccNick;
+         int16_t AdWertAccRoll_temp = AdWertAccRoll;
+        SREG = tmp_sreg;
 //DebugOut.Analog[26] = MesswertNick;
 //DebugOut.Analog[28] = MesswertRoll;
 // Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
-        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
+        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick_temp))) / 2L;
-        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
+        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll_temp))) / 2L;
-    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
+    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick_temp;
-    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
+    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll_temp;
 // Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
             Mess_Integral_Gier +=  MesswertGier;
             Mess_Integral_Gier2 += MesswertGier;
 // Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
 //############################################################################
 // Messwerte beim Ermitteln der Nullage
 void CalibrierMittelwert(void)
 //############################################################################
 {
-    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
+        uint8_t tmp_sreg = SREG;
-        ANALOG_OFF;
+    cli();
-        MesswertNick = AdWertNick;
+         MesswertNick = AdWertNick;
-        MesswertRoll = AdWertRoll;
+         MesswertRoll = AdWertRoll;
-        MesswertGier = AdWertGier;
+         MesswertGier = AdWertGier;
-        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
+         Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
-        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
+         Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
-   // ADC einschalten
+    SREG = tmp_sreg;
     ANALOG_ON;
     if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--;
     if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--;
     if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--;
     if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 < 255) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 > 0) Poti4--;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         int16_t PPM_in_nick, PPM_in_roll, PPM_in_gier, PPM_diff_nick, PPM_diff_roll, PPM_in_gas;
         uint8_t tmp_sreg = SREG;
     cli();
-    PPM_in_nick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
+     PPM_in_nick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
-    PPM_diff_nick = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]];
+     PPM_diff_nick = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]];
-    PPM_in_roll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
+     PPM_in_roll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
-    PPM_diff_roll = PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]];
+     PPM_diff_roll = PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]];
-    PPM_in_gier = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
+     PPM_in_gier = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
-    PPM_in_gas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];
+     PPM_in_gas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];
     SREG = tmp_sreg;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(abs(PPM_in_nick) > MaxStickNick)
      MaxStickNick = abs(PPM_in_nick); else MaxStickNick--;
    if(abs(PPM_in_roll) > MaxStickRoll)
      MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--;
-   if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}
+   if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}
     GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
 //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    if(abs(StickGier) > 20)
+    if(abs(StickGier) > 15)
+     {
       if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
+     }
     tmp_int  = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
+        {
-                if (!updKompass--)              // Aufruf mit ~15 Hz
+                if (!updKompass--)              // Aufruf mit ~20 Hz
+                {
-                        updKompass = 33;
+                        updKompass = 25;
                         if ((MaxStickNick < 50) && (MaxStickRoll < 50))         // Bei extremen Flugmanövern keine Kompassauswertung
+                        {
                                 KompassValue = heading_MM3();
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(!TimerWerteausgabe--)
+        {
         TimerWerteausgabe = 24;
-        //DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
+        DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
-    //DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
+    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
     DebugOut.Analog[2] = Mittelwert_AccNick;
     DebugOut.Analog[3] = Mittelwert_AccRoll;
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[5] = HoehenWert;
     DebugOut.Analog[6] = Mess_Integral_Hoch / 512;

Subversion Repositories FlightCtrl

(root)/branches/MicroMag3_Nick666/trunc/fc.c - Rev 858 → 887