WebSVN - FlightCtrl - Diff - Rev 592 and 593

 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Copyright (c) 04.2007 Holger Buss
 // + Nur für den privaten Gebrauch
 // + www.MikroKopter.com
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
 // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
 // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
 // + bzgl. der Nutzungsbedingungen aufzunehmen.
 // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
 // + Verkauf von Luftbildaufnahmen, usw.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
 // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
 // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
 // + eindeutig als Ursprung verlinkt werden
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
 // + Benutzung auf eigene Gefahr
 // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
 // + mit unserer Zustimmung zulässig
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Redistributions of source code (with or without modifications) must retain the above copyright notice,
 // + this list of conditions and the following disclaimer.
 // +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
 // +     from this software without specific prior written permission.
 // +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
 // +     for non-commercial use (directly or indirectly)
 // +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
 // +     with our written permission
 // +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
 // +     clearly linked as origin
 // +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
 // +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 // +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 // +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 // +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 // +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 // +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include "main.h"
 volatile int  KompassRichtung = 0;
 unsigned char MAX_GAS,MIN_GAS;
 unsigned char Notlandung = 0;
 unsigned char HoehenReglerAktiv = 0;
 long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
+//-------HF-------
+unsigned char LED_Switch = 0;
+unsigned char LED_flash  = 0;
+unsigned char LED_1  = 0;
+unsigned char LED_2  = 0;
+unsigned char LED_2on  = 0;
+//-------HF-------
 float GyroFaktor;
 float IntegralFaktor;
 volatile int  DiffNick,DiffRoll;
 int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
 unsigned char MotorWert[5];
 volatile unsigned char SenderOkay = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0;
 //############################################################################
 //  Nullwerte ermitteln
 void SetNeutral(void)
 //############################################################################
+{
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
         AdNeutralGier = 0;
     Parameter_AchsKopplung1 = 0;
     Parameter_AchsGegenKopplung1 = 0;
     CalibrierMittelwert();
     Delay_ms_Mess(100);
         CalibrierMittelwert();
     if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
      {
       if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+     }
      AdNeutralNick= AdWertNick;
          AdNeutralRoll= AdWertRoll;
          AdNeutralGier= AdWertGier;
      StartNeutralRoll = AdNeutralRoll;
      StartNeutralNick = AdNeutralNick;
     if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
+    {
       NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
           NeutralAccX = abs(Mittelwert_AccNick) / 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]);
           NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
           NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
     }
         Mess_IntegralNick = 0;
     Mess_IntegralNick2 = 0;
     Mess_IntegralRoll = 0;
     Mess_IntegralRoll2 = 0;
     Mess_Integral_Gier = 0;
     MesswertNick = 0;
     MesswertRoll = 0;
     MesswertGier = 0;
     StartLuftdruck = Luftdruck;
     HoeheD = 0;
     Mess_Integral_Hoch = 0;
     KompassStartwert = KompassValue;
     GPS_Neutral();
     beeptime = 50;
         Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
         Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
     ExternHoehenValue = 0;
+}
 //############################################################################
 // Bearbeitet die Messwerte
 void Mittelwert(void)
 //############################################################################
+{
     static signed long tmpl,tmpl2;
     MesswertGier = (signed int) AdNeutralGier - AdWertGier;
     MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
 // Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
             MesswertRoll += tmpl;
             MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
             Mess_IntegralRoll2 += MesswertRoll;
             Mess_IntegralRoll +=  MesswertRoll - LageKorrekturRoll;
             if(Mess_IntegralRoll > Umschlag180Roll)
+            {
              Mess_IntegralRoll  = -(Umschlag180Roll - 10000L);
              Mess_IntegralRoll2 = Mess_IntegralRoll;
             }
             if(Mess_IntegralRoll <-Umschlag180Roll)
+            {
              Mess_IntegralRoll =  (Umschlag180Roll - 10000L);
              Mess_IntegralRoll2 = Mess_IntegralRoll;
             }
             if(AdWertRoll < 15)   MesswertRoll = -1000;
             if(AdWertRoll <  7)   MesswertRoll = -2000;
             if(PlatinenVersion == 10)
+                         {
               if(AdWertRoll > 1010) MesswertRoll = +1000;
               if(AdWertRoll > 1017) MesswertRoll = +2000;
+                         }
                          else
+                         {
               if(AdWertRoll > 2020) MesswertRoll = +1000;
               if(AdWertRoll > 2034) MesswertRoll = +2000;
+                         }
 // Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
             MesswertNick -= tmpl2;
             MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
             Mess_IntegralNick2 += MesswertNick;
             Mess_IntegralNick  += MesswertNick - LageKorrekturNick;
             if(Mess_IntegralNick > Umschlag180Nick)
+            {
              Mess_IntegralNick = -(Umschlag180Nick - 10000L);
              Mess_IntegralNick2 = Mess_IntegralNick;
             }
             if(Mess_IntegralNick <-Umschlag180Nick)
+            {
              Mess_IntegralNick =  (Umschlag180Nick - 10000L);
              Mess_IntegralNick2 = Mess_IntegralNick;
             }
             if(AdWertNick < 15)   MesswertNick = -1000;
             if(AdWertNick <  7)   MesswertNick = -2000;
             if(PlatinenVersion == 10)
+                         {
               if(AdWertNick > 1010) MesswertNick = +1000;
               if(AdWertNick > 1017) MesswertNick = +2000;
+                         }
                          else
+                         {
               if(AdWertNick > 2020) MesswertNick = +1000;
               if(AdWertNick > 2034) MesswertNick = +2000;
+                         }
 //++++++++++++++++++++++++++++++++++++++++++++++++
 // ADC einschalten
     ANALOG_ON;
 //++++++++++++++++++++++++++++++++++++++++++++++++
     Integral_Gier  = Mess_Integral_Gier;
     IntegralNick = Mess_IntegralNick;
     IntegralRoll = Mess_IntegralRoll;
     IntegralNick2 = Mess_IntegralNick2;
     IntegralRoll2 = Mess_IntegralRoll2;
   if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
   {
     if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);
     else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
     if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);
     else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
+  }
     if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
     if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 //############################################################################
 // Messwerte beim Ermitteln der Nullage
 void CalibrierMittelwert(void)
 //############################################################################
+{
     // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
         ANALOG_OFF;
         MesswertNick = AdWertNick;
         MesswertRoll = AdWertRoll;
         MesswertGier = AdWertGier;
         Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
         Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
         Mittelwert_AccHoch = (long)AdWertAccHoch;
    // ADC einschalten
     ANALOG_ON;
     if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
     if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
     if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
     if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 //############################################################################
 // Senden der Motorwerte per I2C-Bus
 void SendMotorData(void)
 //############################################################################
+{
     if(MOTOR_OFF || !MotorenEin)
+        {
         Motor_Hinten = 0;
         Motor_Vorne = 0;
+        }
     DebugOut.Analog[12] = Motor_Vorne;
     DebugOut.Analog[13] = Motor_Hinten;
     DebugOut.Analog[14] = Motor_Links;
     DebugOut.Analog[15] = Motor_Rechts;
     //Start I2C Interrupt Mode
     twi_state = 0;
     motor = 0;
     i2c_start();
+}
 //############################################################################
 // Trägt ggf. das Poti als Parameter ein
  CHK_POTI(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(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
  CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,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_UserParam5,EE_Parameter.UserParam5,0,255);
  CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,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_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
  CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
          static unsigned int  modell_fliegt = 0;
      static int hoehenregler = 0;
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
      static long ausgleichNick, ausgleichRoll;
         Mittelwert();
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
     if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
     if(GasMischanteil < 0) GasMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Emfang schlecht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
         {
         if(!PcZugriff)
          {
            if(BeepMuster == 0xffff)
+            {
              beeptime = 15000;
              BeepMuster = 0x0c00;
+            }
          }
         if(RcLostTimer) RcLostTimer--;
         else
+         {
           MotorenEin = 0;
           Notlandung = 0;
+         }
         ROT_ON;
         if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
+            {
             GasMischanteil = EE_Parameter.NotGas;
             PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
             PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
+            }
          else MotorenEin = 0;
+        }
         else
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Emfang gut
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(SenderOkay > 140)
+            {
             Notlandung = 0;
             RcLostTimer = EE_Parameter.NotGasZeit * 50;
             if(GasMischanteil > 40)
+                }
             if((modell_fliegt < 200) || (GasMischanteil < 40))
+                {
                 SummeNick = 0;
                 SummeRoll = 0;
                 Mess_Integral_Gier = 0;
                 Mess_Integral_Gier2 = 0;
+                }
             if((GasMischanteil > 200) && MotorenEin == 0)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // auf Nullwerte kalibrieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
+                    {
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                         GRN_OFF;
                          eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
+                        }
                         if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                          {
                              if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
                           }
                             ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
                         SetNeutral();
                         Piep(GetActiveParamSetNumber());
                         }
+                    }
                  else
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
+                    {
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                         GRN_OFF;
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
                         Piep(GetActiveParamSetNumber());
                         }
+                    }
                  else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gas ist unten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
             if(GasMischanteil < 35)
+                {
                 // Starten
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
+                    {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                     if(++delay_einschalten > 200)
+                        {
                         delay_einschalten = 200;
                         modell_fliegt = 1;
                         MotorenEin = 1;
                         sollGier = 0;
                         Mess_Integral_Gier = 0;
                         Mess_Integral_Gier2 = 0;
                         Mess_IntegralNick = 0;
                         Mess_IntegralRoll = 0;
                         Mess_IntegralNick2 = IntegralNick;
                         Mess_IntegralRoll2 = IntegralRoll;
                         SummeNick = 0;
                         SummeRoll = 0;
                         }
                     }
                     else delay_einschalten = 0;
                 //Auf Neutralwerte setzen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Auschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
+                    {
                     if(++delay_ausschalten > 200)  // nicht sofort
+                        {
                         MotorenEin = 0;
                         delay_ausschalten = 200;
                         modell_fliegt = 0;
                         }
+                    }
                 else delay_ausschalten = 0;
+                }
+            }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // neue Werte von der Funke
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!NewPpmData-- || Notlandung)
+  {
     int tmp_int;
     ParameterZuordnung();
     StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
     StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
     StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
     StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
     GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
+    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
+    //------HF------- LEDs Schalten - LED1 Normal, LED2 Blitzlicht
+        LED_Switch = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]];
+        if (((LED_Switch > 32) & (LED_Switch < 96)) | (LED_Switch > 160)) LED_2++; else if (LED_2 > 0) LED_2--;
+        if ((LED_Switch > 96)) LED_1++; else if (LED_1 > 0) LED_1--;
+        if (LED_1 >= 10) {LED_1 = 10; LED1_ON;}
+    if (LED_1 <=  1) LED1_OFF;
+        if (LED_2 >= 10) {LED_2 = 10; LED_2on = 1;}
+        if (LED_2 <=  1) LED_2on = 0;
+        if (LED_2on  ==  1) LED_flash++;
+        if (LED_flash > 77) LED2_ON; else LED2_OFF;
+        if (LED_flash > 80) LED_flash = 0;
-    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
+        //------HF------- End LEDs Schalten
 #define KEY_VALUE (Parameter_UserParam1 * 4)  //(Poti3 * 8)
 if(DubWiseKeys[1]) beeptime = 10;
     if(DubWiseKeys[1] & DUB_KEY_UP)    tmp_int = KEY_VALUE;   else
     if(GyroFaktor < 0) GyroFaktor = 0;
     if(IntegralFaktor < 0) IntegralFaktor = 0;
     // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
     if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
+     {
       if(IntegralNick >  60000)
       {
        StickNick -=  8 * EE_Parameter.Stick_P;
        if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;
       }
       else
       if(IntegralNick < -60000)
+      {
        StickNick += 8 * EE_Parameter.Stick_P;
        if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;
       }
       if(IntegralRoll >  60000)
       {
        StickRoll -=  8 * EE_Parameter.Stick_P;
        if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;
       }
       else
       if(IntegralRoll < -60000)
+      {
        StickRoll += 8 * EE_Parameter.Stick_P;
        if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;
       }
+     }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if((PPM_in[EE_Parameter.Kanalbelegung[K_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[EE_Parameter.Kanalbelegung[K_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[EE_Parameter.Kanalbelegung[K_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[EE_Parameter.Kanalbelegung[K_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(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
    if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
   } // Ende neue Funken-Werte
   if(Looping_Roll) beeptime = 100;
   if(Looping_Roll || Looping_Nick)
-   {
+   {
     if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
    }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Bei Empfangsausfall im Flug
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(Notlandung)
+    {
      StickGier = 0;
      StickNick = 0;
      StickRoll = 0;
      GyroFaktor  = 0.1;
      IntegralFaktor = 0.005;
      Looping_Roll = 0;
      Looping_Nick = 0;
+    }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     ZaehlMessungen = 0;
     LageKorrekturNick = 0;
     LageKorrekturRoll = 0;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!Looping_Nick && !Looping_Roll)
+  {
    long tmp_long, tmp_long2;
     tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
     tmp_long /= 16;
     tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
     tmp_long2 /= 16;
  #define AUSGLEICH 32 //(Parameter_UserParam1 / 2)
     if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
     if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
     if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
     if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
     Mess_IntegralNick -= tmp_long;
     Mess_IntegralRoll -= tmp_long2;
   }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(ZaehlMessungen >= ABGLEICH_ANZAHL)
+ {
 #define MAX_I 0//(Poti2/10)
 // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
     ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
     LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
 // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
     ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
     LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
 //    Mess_IntegralNick -= ausgleichNick;
 //    Mess_IntegralRoll -= ausgleichRoll;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gyro-Drift ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     MittelIntegralNick2 /= ABGLEICH_ANZAHL;
     MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
 //    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
 //    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
     tmp_long  = IntegralNick2 - IntegralNick;
     tmp_long2 = IntegralRoll2 - IntegralRoll;
     //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
 #define BEWEGUNGS_LIMIT 20000
 // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
         cnt = 1;// + labs(IntegralFehlerNick) / 4096;
         if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)
+        {
         if(IntegralFehlerNick >  FEHLER_LIMIT2)
+         {
            if(last_n_p)
+           {
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
             ausgleichNick = IntegralFehlerNick / 8;
             if(ausgleichNick > 5000) ausgleichNick = 5000;
             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
            }
            else last_n_p = 1;
          } else  last_n_p = 0;
         if(IntegralFehlerNick < -FEHLER_LIMIT2)
+         {
            if(last_n_n)
             {
              cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
              ausgleichNick = IntegralFehlerNick / 8;
              if(ausgleichNick < -5000) ausgleichNick = -5000;
              LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
             }
            else last_n_n = 1;
          } else  last_n_n = 0;
         } else cnt = 0;
         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
         if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
         cnt = 1;// + labs(IntegralFehlerNick) / 4096;
         ausgleichRoll = 0;
         if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
         {
         if(IntegralFehlerRoll >  FEHLER_LIMIT2)
          {
            if(last_r_p)
            {
             cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
             ausgleichRoll = IntegralFehlerRoll / 8;
             if(ausgleichRoll > 5000) ausgleichRoll = 5000;
             LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
+           }
            else last_r_p = 1;
          } else  last_r_p = 0;
         if(IntegralFehlerRoll < -FEHLER_LIMIT2)
          {
            if(last_r_n)
            {
             cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
             ausgleichRoll = IntegralFehlerRoll / 8;
             if(ausgleichRoll < -5000) ausgleichRoll = -5000;
             LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
+           }
            else last_r_n = 1;
          } else  last_r_n = 0;
         } else
+        {
          cnt = 0;
+        }
 DebugOut.Analog[27] = ausgleichRoll;
         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 //if(cnt > 1) beeptime = 50;
         if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
         if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
 DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
 DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
   }
   else
+  {
    LageKorrekturRoll = 0;
    LageKorrekturNick = 0;
+  }
   if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++
    MittelIntegralNick_Alt = MittelIntegralNick;
    MittelIntegralRoll_Alt = MittelIntegralRoll;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralAccNick = 0;
     IntegralAccRoll = 0;
     IntegralAccZ = 0;
     MittelIntegralNick = 0;
     MittelIntegralRoll2 = 0;
     ZaehlMessungen = 0;
+ }
 //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(abs(StickGier) > 20) // war 35
+     {
       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²
     tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
     sollGier = 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;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+     {
        int w,v;
        static int SignalSchlecht = 0;
        w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
        v = abs(IntegralRoll /512);
        if(v > w) w = v; // grösste Neigung ermitteln
        if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)
+        {
          KompassStartwert = KompassValue;
          NeueKompassRichtungMerken = 0;
+        }
        w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
        if(w > 0)
+        {
           if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
           if(SignalSchlecht) SignalSchlecht--;
+        }
         else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
      }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!TimerWerteausgabe--)
+   {
     TimerWerteausgabe = 24;
 */
 //    DebugOut.Analog[9] = MesswertNick;
 //    DebugOut.Analog[9] = SollHoehe;
 //    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 //    DebugOut.Analog[11] = KompassStartwert;
 //    DebugOut.Analog[10] = Parameter_Gyro_I;
 //    DebugOut.Analog[10] = EE_Parameter.Gyro_I;
 //    DebugOut.Analog[9] = KompassRichtung;
 //    DebugOut.Analog[10] = GasMischanteil;
 //    DebugOut.Analog[3] = HoeheD * 32;
 //    DebugOut.Analog[4] = hoehenregler;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
     else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
     if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
     else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
     if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
     if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
     if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
     if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Höhenregelung
 // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //OCR0B = 180 - (Poti1 + 120) / 4;
 //DruckOffsetSetting = OCR0B;
  if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
+  {
     int tmp_int;
     if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
     {
      if(Parameter_MaxHoehe < 50)
+      {
        SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
        HoehenReglerAktiv = 0;
       }
       else
         HoehenReglerAktiv = 1;
     }
     else
+    {
      SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
      HoehenReglerAktiv = 1;
       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;
       if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
+       {
          if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
          if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
        }
       if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
       GasMischanteil = hoehenregler;
      }
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   DebugOut.Analog[7] = GasMischanteil;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier-Anteil
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define MUL_G  1.0
     GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 //GierMischanteil = 0;
     if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
     if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
     if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil));
     if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS - GasMischanteil));
     if(GasMischanteil < 20) GierMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = MesswertNick - (StickNick - GPS_Nick);   // Differenz bestimmen
     if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
     else  SummeNick += DiffNick; // I-Anteil bei HH
     if(SummeNick > 0) SummeNick-= 2 ; else SummeNick += 2 ;
     if(SummeNick >  16000) SummeNick =  16000;
     if(SummeNick < -16000) SummeNick = -16000;
     pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
     // Motor Vorn
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
     if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
     motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor_Vorne = motorwert;
     // Motor Heck
         motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor_Hinten = motorwert;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = MesswertRoll - (StickRoll  - GPS_Roll);      // Differenz bestimmen
     if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll  - GPS_Roll);// I-Anteil bei Winkelregelung
     else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
     if(SummeRoll > 0) SummeRoll-= 2 ; else SummeRoll += 2 ;
     if(SummeRoll >  16000) SummeRoll =  16000;
     if(SummeRoll < -16000) SummeRoll = -16000;
     pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
     if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
     // Motor Links
     motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor_Links = motorwert;
     // Motor Rechts
         motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;

Subversion Repositories FlightCtrl

(root)/branches/V0.67g_LED/fc.c @ 1221 - Rev 592 → 593