WebSVN - FlightCtrl - Diff - Rev 1036 and 1051 - /branches/Proxy sensor

 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + 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"
 #include "eeprom.c"
 unsigned char h,m,s;
 volatile unsigned int I2CTimeout = 100;
 volatile int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias;
 int AdNeutralGierBias;
 int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
 int   ErsatzKompassInGrad; // Kompasswert in Grad
 int   GierGyroFehler = 0;
 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;
 volatile unsigned char SenderOkay = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
 char MotorenEin = 0;
 int HoehenWert = 0;
 unsigned char Parameter_J16Bitmask;             // for the J16 Output
 unsigned char Parameter_J16Timing;              // for the J16 Output
 unsigned char Parameter_J17Bitmask;             // for the J17 Output
 unsigned char Parameter_J17Timing;              // for the J17 Output
 unsigned char Parameter_NaviGpsModeControl;     // Parameters for the Naviboard
 unsigned char Parameter_NaviGpsGain;
 unsigned char Parameter_NaviGpsP;
 unsigned char Parameter_NaviGpsI;
 unsigned char Parameter_NaviGpsD;
 unsigned char Parameter_NaviGpsACC;
 unsigned char Parameter_NaviOperatingRadius;
 unsigned char Parameter_NaviWindCorrection;
 unsigned char Parameter_NaviSpeedCompensation;
 unsigned char Parameter_ExternalControl;
 struct mk_param_struct EE_Parameter;
 //############################################################################
 //  Nullwerte ermitteln
 void SetNeutral(void)
 //############################################################################
+{
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
         AdNeutralGier = 0;
     AdNeutralGierBias = 0;
     Parameter_AchsKopplung1 = 0;
     Parameter_AchsGegenKopplung1 = 0;
     ExpandBaro = 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;
      AdNeutralGierBias = 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;
  Delay_ms_Mess(100);
     StartLuftdruck = Luftdruck;
     HoeheD = 0;
     Mess_Integral_Hoch = 0;
     KompassStartwert = KompassValue;
     GPS_Neutral();
     beeptime = 50;
         Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
         Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
     ExternHoehenValue = 0;
     ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
 //############################################################################
 // Bearbeitet die Messwerte
 void Mittelwert(void)
 //############################################################################
 {
     static signed long tmpl,tmpl2;
     MesswertGier = (signed int) AdNeutralGier - AdWertGier;
     MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
     MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
             tmpl *= Parameter_AchsKopplung1;  //125
             tmpl /= 4096L;
             tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L;
             tmpl2 *= Parameter_AchsKopplung1;
             tmpl2 /= 4096L;
             if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
+         }
       else  tmpl = tmpl2 = 0;
 // Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
             MesswertRoll += tmpl;
             MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
             Mess_IntegralRoll2 += MesswertRoll;
             Mess_IntegralRoll +=  MesswertRoll - LageKorrekturRoll;
             if(Mess_IntegralRoll > Umschlag180Roll)
+            {
              Mess_IntegralRoll  = -(Umschlag180Roll - 25000L);
              Mess_IntegralRoll2 = Mess_IntegralRoll;
             }
             if(Mess_IntegralRoll <-Umschlag180Roll)
+            {
              Mess_IntegralRoll =  (Umschlag180Roll - 25000L);
              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 - 25000L);
               Mess_IntegralNick2 = Mess_IntegralNick;
              }
             if(Mess_IntegralNick <-Umschlag180Nick)
+            {
              Mess_IntegralNick =  (Umschlag180Nick - 25000L);
              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--;
 //############################################################################
 // Messwerte beim Ermitteln der Nullage
 void CalibrierMittelwert(void)
 //############################################################################
+{
     if(PlatinenVersion == 13) SucheGyroOffset();
     // 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(!MotorenEin)
+        {
         Motor_Hinten = 0;
         Motor_Vorne = 0;
         } else MikroKopterFlags |= FLAG_MOTOR_RUN;
     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
 void ParameterZuordnung(void)
  CHK_POTI_MM(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_MM(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 char delay_einschalten = 0,delay_ausschalten = 0;
      static int hoehenregler = 0;
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
      static long ausgleichNick, ausgleichRoll;
         Mittelwert();
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = StickGas;
     if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Empfang 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 > 1000)  // wahrscheinlich in der Luft --> langsam absenken
+            {
             GasMischanteil = EE_Parameter.NotGas;
             Notlandung = 1;
             PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
             PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
             PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
             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((modell_fliegt < 256))
+                {
                 SummeNick = 0;
                 SummeRoll = 0;
                 if(modell_fliegt == 250)
+                 {
                   NeueKompassRichtungMerken = 1;
                   sollGier = 0;
                   Mess_Integral_Gier = 0;
 //                  Mess_Integral_Gier2 = 0;
                  }
                 } else MikroKopterFlags |= FLAG_FLY;
             if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // auf Nullwerte kalibrieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
+                    {
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                         GRN_OFF;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
                          eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
+                        }
 //                        else
                          if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
                           {
                            WinkelOut.CalcState = 1;
                            beeptime = 1000;
+                          }
                           else
+                          {
                                ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
                            if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                            {
                              if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
                             }
                            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(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
+                {
                 // 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;
                         MikroKopterFlags |= FLAG_START;
                         }
                     }
                     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;
         static int stick_nick,stick_roll;
     ParameterZuordnung();
     stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
     stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
     StickNick = stick_nick - (GPS_Nick + GPS_Nick2);
 //    StickNick  = (StickNick  * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
     stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
     stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
     StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
 //    StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
         StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 /*   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick)
      MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--;
    if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll)
-     MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;
-*/
-    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / (256.0/STICK_GAIN);
-    IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
-//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//+ Digitale Steuerung per DubWise
-//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-#define KEY_VALUE (Parameter_ExternalControl * 4)  //(Poti3 * 8)
-if(DubWiseKeys[1]) beeptime = 10;
-    if(DubWiseKeys[1] & DUB_KEY_UP)    tmp_int = KEY_VALUE;   else
-    if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;  else   tmp_int = 0;
-    ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
-    if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE; else
-    if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0;
-    ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
-    if(DubWiseKeys[0] & 8)  ExternStickGier = 50;else
-    if(DubWiseKeys[0] & 4)  ExternStickGier =-50;else ExternStickGier = 0;
-    if(DubWiseKeys[0] & 2)  ExternHoehenValue++;
-    if(DubWiseKeys[0] & 16) ExternHoehenValue--;
+     MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;
 */
-    StickNick += (STICK_GAIN * ExternStickNick) / 8;
+    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / (256.0/STICK_GAIN);
-    StickRoll += (STICK_GAIN * ExternStickRoll) / 8;
+    IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
     StickGier += STICK_GAIN * ExternStickGier;
     if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
     if(GyroFaktor < 0) GyroFaktor = 0;
     if(IntegralFaktor < 0) IntegralFaktor = 0;
     if(abs(StickNick/STICK_GAIN) > MaxStickNick)
+     {
       MaxStickNick = abs(StickNick)/STICK_GAIN;
       if(MaxStickNick > 100) MaxStickNick = 100;
      }
      else MaxStickNick--;
     if(abs(StickRoll/STICK_GAIN) > MaxStickRoll)
+     {
       MaxStickRoll = abs(StickRoll)/STICK_GAIN;
       if(MaxStickRoll > 100) MaxStickRoll = 100;
+     }
      else MaxStickRoll--;
     if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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.BitConfig & 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.BitConfig & 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.BitConfig & 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
+  } // Ende neue Funken-Werte
   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     = (float) 100 / (256.0 / STICK_GAIN);
      IntegralFaktor = (float) 120 / (44000 / STICK_GAIN);
      Looping_Roll = 0;
      Looping_Nick = 0;
+    }
     ZaehlMessungen = 0;
     LageKorrekturNick = 0;
     LageKorrekturRoll = 0;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!Looping_Nick && !Looping_Roll)
+  {
    long tmp_long, tmp_long2;
    if(FromNaviCtrl_Value.Kalman_K != -1)
+     {
       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 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
      if((MaxStickNick > 64) || (MaxStickRoll > 64))
+      {
       tmp_long  /= 2;
       tmp_long2 /= 2;
       if(tmp_long >  (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
       if(tmp_long <  (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
       if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
       if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
+     }
      else
+     {
       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 > 64) || (MaxStickRoll > 64))
+      {
       tmp_long  /= 3;
       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)
+ {
   static int cnt = 0;
   static char last_n_p,last_n_n,last_r_p,last_r_n;
   static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
     IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
 #define MAX_I 0//(Poti2/10)
 // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
     ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
 // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
     ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
     LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
+    {
      LageKorrekturNick /= 2;
      LageKorrekturRoll /= 2;
+    }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gyro-Drift ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     MittelIntegralNick2 /= ABGLEICH_ANZAHL;
     MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
     tmp_long  = IntegralNick2 - IntegralNick;
     tmp_long2 = IntegralRoll2 - IntegralRoll;
     //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
     IntegralFehlerNick = tmp_long;
 #define BEWEGUNGS_LIMIT 20000
 // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
         cnt = 1;// + labs(IntegralFehlerNick) / 4096;
         if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))
+        {
         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;
          KompassSignalSchlecht = 1000;
         }
         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
         if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;
         if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
         if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
         cnt = 1;// + labs(IntegralFehlerNick) / 4096;
         ausgleichRoll = 0;
         if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))
         {
         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;
          KompassSignalSchlecht = 1000;
+        }
         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
         if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;
         if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
         if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
   }
   else
+  {
    LageKorrekturRoll = 0;
    LageKorrekturNick = 0;
    TrichterFlug = 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(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
     if(abs(StickGier) > 15) // war 35
+     {
       KompassSignalSchlecht = 1000;
       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
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
     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)
+            {
                  fehler = 0;
                  ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
+                }
        if(!KompassSignalSchlecht && w < 25)
         {
         GierGyroFehler += fehler;
         if(NeueKompassRichtungMerken)
+         {
           beeptime = 200;
 //         KompassStartwert = KompassValue;
           KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
        ErsatzKompass += (fehler * 8) / korrektur;
        w = (w * Parameter_KompassWirkung) / 32;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
        if(w >= 0)
+        {
           if(!KompassSignalSchlecht)
+          {
            v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
            r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
 //           r = KompassRichtung;
            v = (r * w) / v;  // nach Kompass ausrichten
            w = 3 * Parameter_KompassWirkung;
            if(v > w) v = w; // Begrenzen
            else
            if(v < -w) v = -w;
            Mess_Integral_Gier += v;
           }
           if(KompassSignalSchlecht) KompassSignalSchlecht--;
         }
         else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
      }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!TimerWerteausgabe--)
+   {
     TimerWerteausgabe = 24;
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[5] = HoehenWert;
     DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
     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[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(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;
   GasMischanteil *= STICK_GAIN;
 /*
     if(((EE_Parameter.BitConfig & CFG_HIGHT_3SWITCH) && ((Parameter_MaxHoehe > 80) && (Parameter_MaxHoehe < 140))) ||
       (!(EE_Parameter.BitConfig & CFG_HIGHT_3SWITCH) && (Parameter_MaxHoehe < 50)))
+      {
        if(!delay--)
+            {
          if(MessLuftdruck > 1000)
+                  {
                    ExpandBaro -= 10;
                    OCR0A = DruckOffsetSetting - ExpandBaro;
                    beeptime = 300;
            delay = 250;
+                  }
                   else
          if(MessLuftdruck < 100)
+                  {
                    ExpandBaro += 10;
                    OCR0A = DruckOffsetSetting - ExpandBaro;
                    beeptime = 300;
            delay = 250;
+                  }
           else
           {
                SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
            HoehenReglerAktiv = 0;
                    delay = 1;
+                  }
                 }
+      }
       else
         {
          HoehenReglerAktiv = 1;
                  delay = 200;
                 }
+    }
     else
+    {
      SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
      HoehenReglerAktiv = 1;
+    }
     if(Notlandung) SollHoehe = 0;
     h = HoehenWert;
     if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
+     {
       h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
       h = GasMischanteil - h;         // vom Gas abziehen
 //      h -= (HoeheD * Parameter_Luftdruck_D)/(8/STICK_GAIN);    // D-Anteil
       h -= (HoeheD)/(8/STICK_GAIN);    // D-Anteil
       tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
       if(tmp_int > 70*STICK_GAIN) tmp_int = 70*STICK_GAIN;
       else if(tmp_int < -(70*STICK_GAIN)) tmp_int = -(70*STICK_GAIN);
       h -= tmp_int;
       hoehenregler = (hoehenregler*15 + h) / 16;
       if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
+       {
          if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
          if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
+       }
       if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
       GasMischanteil = hoehenregler;
+     }
+  }
   if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   DebugOut.Analog[7] = GasMischanteil;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier-Anteil
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define MUL_G  1.0
     GierMischanteil = MesswertGier - sollGier * STICK_GAIN;     // Regler für Gier
 // GierMischanteil = 0;
 #define MIN_GIERGAS  (40*STICK_GAIN)  // unter diesem Gaswert trotzdem Gieren
    if(GasMischanteil > MIN_GIERGAS)
     {
      if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
      if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
     }
     else
     {
      if(GierMischanteil > (MIN_GIERGAS / 2))  GierMischanteil = MIN_GIERGAS / 2;
      if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
+    }
     tmp_int = MAX_GAS*STICK_GAIN;
     if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
     if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
     if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
     else  SummeNick += DiffNick; // I-Anteil bei HH
     if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
     if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
     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
     motorwert /= STICK_GAIN;
         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;
     motorwert /= STICK_GAIN;
         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;    // Differenz bestimmen
     if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung
     else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
     if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
     if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
     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;
     motorwert /= STICK_GAIN;
         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;
     motorwert /= STICK_GAIN;
         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/Proxy sensor - tempolo/fc.c @ 2291 - Rev 1036 → 1051