WebSVN - FlightCtrl - Diff - Rev 787 and 851 - /branches/MicroMag3

         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[EE_Parameter.Kanalbelegung[K_GAS]] > 100) //Warten, bis Benutzer den Kopter neu ausgerichtet hat
+        while (PPM_in_gas > 100)        //Warten, bis Benutzer den Kopter neu ausgerichtet hat
+        {
                 uint8_t delay=9;
                         beeptime = 100;
+                }
+        }
         Delay_ms_Mess(100);
         acc_neutral.C = Aktuell_az;
+*/
         eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));
+}
 //############################################################################
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = StickGas;
     if(GasMischanteil < 0) GasMischanteil = 0;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Datenübernahme von RC
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        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_diff_nick = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]];
+    PPM_in_roll = PPM_in[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_gas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];
+    SREG = tmp_sreg;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Emfang schlecht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
+        {
         ROT_ON;
         if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
+                {
                         GasMischanteil = EE_Parameter.NotGas;
                         Notlandung = 1;
-                        PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
+                        PPM_diff_nick = 0;
-                        PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
+                        PPM_diff_roll = 0;
-                        PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
+                        PPM_in_nick = 0;
-                        PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
+                        PPM_in_roll = 0;
-                        PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
+                        PPM_in_gier = 0;
+                }
         else MotorenEin = 0;
+        }
         else
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 SummeNick = 0;
                 SummeRoll = 0;
                 Mess_Integral_Gier = 0;
                 Mess_Integral_Gier2 = 0;
+                }
-            if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
+            if((PPM_in_gas > 80) && MotorenEin == 0)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // auf Nullwerte kalibrieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
+                if(PPM_in_gier > 75)  // Neutralwerte
+                    {
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                         GRN_OFF;
                         MotorenEin = 0;
                         delay_neutral = 0;
                         modell_fliegt = 0;
-                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
+                        if(PPM_in_nick > 70 || abs(PPM_in_roll) > 70)
+                        {
                          unsigned char setting=1;
-                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
+                         if(PPM_in_roll > 70 && PPM_in_nick < 70) setting = 1;
-                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
+                         if(PPM_in_roll > 70 && PPM_in_nick > 70) setting = 2;
-                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
+                         if(PPM_in_roll < 70 && PPM_in_nick > 70) setting = 3;
-                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
+                         if(PPM_in_roll <-70 && PPM_in_nick > 70) setting = 4;
-                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
+                         if(PPM_in_roll <-70 && PPM_in_nick < 70) setting = 5;
                          eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
+                        }
                             ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], sizeof(struct mk_param_struct));
                         SetNeutral();
                         Piep(GetActiveParamSetNumber());
                  else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gas ist unten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-            if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
+            if(PPM_in_gas < 35-120)
+                {
                 // Starten
-                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
+                if(PPM_in_gier < -75)
+                    {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                     if(++delay_einschalten > 200)
                     else delay_einschalten = 0;
                 //Auf Neutralwerte setzen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Auschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
+                if(PPM_in_gier > 75)
+                    {
                     if(++delay_ausschalten > 200)  // nicht sofort
+                        {
                         MotorenEin = 0;
                         delay_ausschalten = 200;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!NewPpmData-- || Notlandung)
+  {
     int tmp_int;
     ParameterZuordnung();
-    StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
+    StickNick = (StickNick * 3 + PPM_in_nick * EE_Parameter.Stick_P) / 4;
-    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
+    StickNick += PPM_diff_nick * EE_Parameter.Stick_D;
-    StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
+    StickRoll = (StickRoll * 3 + PPM_in_roll * EE_Parameter.Stick_P) / 4;
-    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
+    StickRoll += PPM_diff_roll * EE_Parameter.Stick_D;
-    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
+    StickGier = -PPM_in_gier;
-        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
+        StickGas  = PPM_in_gas + 120;
-   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick)
+   if(abs(PPM_in_nick) > MaxStickNick)
-     MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--;
+     MaxStickNick = abs(PPM_in_nick); else MaxStickNick--;
-   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll)
+   if(abs(PPM_in_roll) > MaxStickRoll)
-     MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;
+     MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--;
    if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}
     GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
     if(GyroFaktor < 0) GyroFaktor = 0;
     if(IntegralFaktor < 0) IntegralFaktor = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
+  if((PPM_in_roll > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
   else
+   {
+     {
-      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;
+      if((PPM_in_roll < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;
+     }
+   }
-  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
+  if((PPM_in_roll < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
    else
+   {
    if(Looping_Rechts) // Hysterese
+     {
-      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
+      if(PPM_in_roll > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
+     }
+   }
-  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
+  if((PPM_in_nick > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
   else
+   {
     if(Looping_Oben)  // Hysterese
      {
-      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;
+      if((PPM_in_nick < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;
+     }
    }
-  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
+  if((PPM_in_nick < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
    else
+   {
     if(Looping_Unten) // Hysterese
      {
-      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
+      if(PPM_in_nick > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
+     }
+   }
    if(Notlandung)
+    {
      StickGier = 0;
      StickNick = 0;
      StickRoll = 0;
-     GyroFaktor  = 0.1;
+     GyroFaktor  = 0.5;
-     IntegralFaktor = 0.005;
+     IntegralFaktor = 0.003;
      Looping_Roll = 0;
      Looping_Nick = 0;
+    }
    long tmp_long, tmp_long2;
     tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
     tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
     tmp_long /= 16;
     tmp_long2 /= 16;
-   if((MaxStickNick > 15) || (MaxStickRoll > 15))
+   if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in_gier) > 25))
-    {
-    tmp_long  /= 3;
-    tmp_long2 /= 3;
-    }
-   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
+    {
     tmp_long  /= 3;
     tmp_long2 /= 3;
+    }
+   //if(abs(PPM_in_gier) > 25)
+   // {
+   // tmp_long  /= 3;
+   // tmp_long2 /= 3;
+   // }
  #define AUSGLEICH 32
     if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
     if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
     ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
     LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
     LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
-   if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))
+   if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in_gier) > 25))
+    {
      LageKorrekturNick /= 2;
+        {
                 if (!updKompass--)              // Aufruf mit ~10 Hz
+                {
                         updKompass = 49;
-                        if ((MaxStickNick < 75) && (MaxStickRoll < 75))         // Bei extremen Flugmanövern keine Kompassauswertung
+                        if ((MaxStickNick < 50) && (MaxStickRoll < 50))         // Bei extremen Flugmanövern keine Kompassauswertung
+                        {
                                 KompassValue = heading_MM3();
                                 KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
                                 if(NeueKompassRichtungMerken)
     if(Notlandung) SollHoehe = 0;
     h = HoehenWert;
     if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
      {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
       h = GasMischanteil - h;         // vom Gas abziehen
-      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
+      h -= (HoeheD * Parameter_Luftdruck_D) / 8;    // D-Anteil
       tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
       if(tmp_int > 50) tmp_int = 50;
       else if(tmp_int < -50) tmp_int = -50;
       h -= tmp_int;
       hoehenregler = (hoehenregler*15 + h) / 16;

Subversion Repositories FlightCtrl

(root)/branches/MicroMag3_Nick666/trunc/fc.c - Rev 787 → 851