Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

/*****************************************************************************************************************************
* File:                 fc.c
*
* Purpose:              Flight Control
*
* Functions:    int MotorSmoothing(int neu, int alt)
*                               void Piep(unsigned char Anzahl, unsigned int dauer)
*                               void SetNeutral(void)
*                               void Mittelwert(void)
*                               void CalibrierMittelwert(void)
*                               void SendMotorData(void)
*                               void ParameterZuordnung(void)
*                               void MotorRegler(void)
*
*****************************************************************************************************************************/

#include "fc.h"

#include "main.h"
#include "eeprom.c"
#include "mymath.h"
#include "isqrt.h"                      // wird nur gebraucht wegen Zeile 1265 // CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2);

unsigned char h,m,s;
unsigned int BaroExpandActive = 0;
volatile unsigned int I2CTimeout = 100;

int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;

int TrimNick, TrimRoll;
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;

volatile float NeutralAccZ = 0;

unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;

long IntegralNick = 0;                                                                                                                          // dieser Wert wird im 3D Koptertool angezeigt
long IntegralNick2 = 0;                                        
long IntegralRoll = 0;
long IntegralRoll2 = 0;
long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
long Integral_Gier = 0;
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
long MittelIntegralNick, MittelIntegralRoll, MittelIntegralNick2, MittelIntegralRoll2;
volatile long Mess_Integral_Hoch = 0;

int  KompassValue = 0;
int  KompassStartwert = 0;
int  KompassRichtung = 0;
unsigned int  KompassSignalSchlecht = 500;
long  ErsatzKompass;
int   ErsatzKompassInGrad;                                                                                                                                      // Kompasswert in Grad

char MotorenEin = 0;
unsigned char  MAX_GAS,MIN_GAS;

unsigned char HoehenReglerAktiv = 0;
unsigned char TrichterFlug = 0;
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
int   GierGyroFehler = 0;
char GyroFaktor,GyroFaktorGier;
char IntegralFaktor,IntegralFaktorGier;
int  DiffNick,DiffRoll;
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;

volatile unsigned char SenderOkay = 0;
volatile unsigned char SenderRSSI = 0;

int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;                                                             // Stick aus der Fernsteuerung

long HoehenWert = 0;
long SollHoehe = 0;
int LageKorrekturRoll = 0,LageKorrekturNick = 0;

int Ki = 10300 / 33;

unsigned char Looping_Nick = 0,Looping_Roll = 0;
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;

unsigned char Parameter_Luftdruck_D  = 48;                      // Wert : 0-250
unsigned char Parameter_MaxHoehe     = 251;                     // Wert : 0-250
unsigned char Parameter_Hoehe_P      = 16;                      // Wert : 0-32
unsigned char Parameter_Hoehe_ACC_Wirkung = 58;                 // Wert : 0-250
unsigned char Parameter_KompassWirkung = 64;                    // Wert : 0-250
unsigned char Parameter_Hoehe_GPS_Z = 64;                       // Wert : 0-250

unsigned char Parameter_Gyro_D = 8;                             // Wert : 0-250
unsigned char Parameter_Gyro_P = 150;                           // Wert : 10-250
unsigned char Parameter_Gyro_I = 150;                           // Wert : 0-250
unsigned char Parameter_Gyro_Gier_P = 150;                      // Wert : 10-250
unsigned char Parameter_Gyro_Gier_I = 150;                      // Wert : 10-250
unsigned char Parameter_Gier_P = 2;                             // Wert : 1-20
unsigned char Parameter_I_Faktor = 10;                          // Wert : 1-20

unsigned char Parameter_UserParam1 = 0;
unsigned char Parameter_UserParam2 = 0;
unsigned char Parameter_UserParam3 = 0;
unsigned char Parameter_UserParam4 = 0;
unsigned char Parameter_UserParam5 = 0;
unsigned char Parameter_UserParam6 = 0;
unsigned char Parameter_UserParam7 = 0;
unsigned char Parameter_UserParam8 = 0;
unsigned char Parameter_ServoNickControl = 100;
unsigned char Parameter_ServoRollControl = 100;
unsigned char Parameter_LoopGasLimit = 70;
unsigned char Parameter_AchsKopplung1 = 90;
unsigned char Parameter_AchsKopplung2 = 65;
unsigned char Parameter_CouplingYawCorrection = 64;
unsigned char Parameter_DynamicStability = 100;
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;                    // definiert in fc.h Zeile 80

signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;

int MaxStickNick = 0,MaxStickRoll = 0;
unsigned int  modell_fliegt = 0;
volatile unsigned char MikroKopterFlags = 0;
long GIER_GRAD_FAKTOR = 1291;
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
unsigned char RequiredMotors = 4;
unsigned char Motor[MAX_MOTORS];
signed int tmp_motorwert[MAX_MOTORS];
unsigned char LoadHandler = 0;

//------------------------------------------------------------ Definitionen --------------------------------------
#define LIMIT_MIN(value, min) {if(value < min) value = min;}
#define LIMIT_MAX(value, max) {if(value > max) value = max;}
#define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}


// *****************************************************************************************************************
int MotorSmoothing(int neu, int alt)
{
        int motor;
        if(neu > alt) motor = (1*(int)alt + neu) / 2;
        else   motor = neu - (alt - neu)*1;
        return(motor);
}
//------------------------------------------------------


// ***********************************************************
// erzeugt einen Piepton
//------------------------------------------------------
void Piep(unsigned char Anzahl, unsigned int dauer)
{
        if(MotorenEin) return; //auf keinen Fall im Flug!
        while(Anzahl--)
        {
                beeptime = dauer;
                while(beeptime);
                Delay_ms(dauer * 2);
        }
}
//------------------------------------------------------



// *******************************************************************************************************************
//  Nullwerte ermitteln und Startwerte festlegen
// -----------------------------------------------
void SetNeutral(void)
{
        unsigned char i;
        unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
    HEF4017R_ON;
        NeutralAccX = 0;
        NeutralAccY = 0;
        NeutralAccZ = 0;
        AdNeutralNick = 0;
        AdNeutralRoll = 0;
        AdNeutralGier = 0;
        AdNeutralGierBias = 0;
        Parameter_AchsKopplung1 = 0;
        Parameter_AchsKopplung2 = 0;
        ExpandBaro = 0;
       
        CalibrierMittelwert();
        Delay_ms_Mess(100);
       
        CalibrierMittelwert();
        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))                                                    // Höhenregelung aktiviert?
        {
                if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
        }
       
        #define NEUTRAL_FILTER 32
       
        for(i=0; i<NEUTRAL_FILTER; i++)
        {
                Delay_ms_Mess(10);
                gier_neutral += AdWertGier;
                nick_neutral += AdWertNick;
                roll_neutral += AdWertRoll;
        }
       
        AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
        AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
        AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);
        AdNeutralGierBias = AdNeutralGier;
        StartNeutralRoll = AdNeutralRoll;
        StartNeutralNick = AdNeutralNick;
       
        if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
        {
                NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);                                        // #define ACC_AMPLIFY    6
                NeutralAccX = abs(Mittelwert_AccNick) / (2*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]);
        }
       
    MesswertNick = 0;
    MesswertRoll = 0;
    MesswertGier = 0;
    Delay_ms_Mess(100);
    Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;                                                     // #define ACC_AMPLIFY    6
    Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
    IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
    IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
    Mess_IntegralNick2 = IntegralNick;
    Mess_IntegralRoll2 = IntegralRoll;
    Mess_Integral_Gier = 0;
    StartLuftdruck = Luftdruck;
    VarioMeter = 0;
    Mess_Integral_Hoch = 0;
    KompassStartwert = KompassValue;
    //GPS_Neutral();                                            // no GPS available
    beeptime = 50;
        Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
        Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
    ExternHoehenValue = 0;
    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;                                                            // long GIER_GRAD_FAKTOR = 1291;
    GierGyroFehler = 0;
    SendVersionToNavi = 1;
    LED_Init();
    MikroKopterFlags |= FLAG_CALIBRATE;

    Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;
    Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;
    Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;
    Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;
    SenderOkay = 100;
    //----------------------------------------------
        if(ServoActive)
        {
                HEF4017R_ON;
                DDRD  |=0x80; // enable J7 -> Servo signal
    }
        //----------------------------------------------
}
//*** EOF: SetNeutral(void) *******************************************************************************************************



// ********************************************************************************************************************************
// Bearbeitet die Messwerte und legt die Mittelwerte fest
// ----------------------------------------------------------
void Mittelwert(void)
{
    static signed long tmpl,tmpl2,tmpl3,tmpl4;
        static signed int oldNick, oldRoll, d2Roll, d2Nick;
        signed long winkel_nick, winkel_roll;

        // MesswertGier = (signed int) AdNeutralGier - AdWertGier;                              // Orginalcode vorher
        MesswertGier = AdWertGier - (signed int) AdNeutralGier;                                 // Gyro ist um 180 Grad gedreht eingebaut

        //    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
    MesswertNick = (signed int) AdWertNickFilter / 8;
    MesswertRoll = (signed int) AdWertRollFilter / 8;
    RohMesswertNick = MesswertNick;
    RohMesswertRoll = MesswertRoll;

        // Beschleunigungssensor  --------------------------------------------------------------------------
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L;               // #define ACC_AMPLIFY    6
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L;
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 3 + ((long)AdWertAccHoch)) / 4L;
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;                                                                                                                     // #define ACC_AMPLIFY    6
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;                                                                                                                     // #define ACC_AMPLIFY    6
    NaviAccNick    += AdWertAccNick;
    NaviAccRoll    += AdWertAccRoll;
    NaviCntAcc++;
    IntegralAccZ  += Aktuell_az - NeutralAccZ;

        //------------------------------------------------------------
        // Single Conversion at ADC
        //
        // ADCSRA = Analog Digital Conversion Status Register A -> ADEN ADSC ADATE ADIF ADIE ADPS2 ADPS1 ADPS0 
        // ---------------------------------------------------------------------------------------------------
        // ADEN  = Writing this bit to one enables the ADC
        // ADSC  = start conversion
        // ADATE = When this bit is written to one, Auto Triggering of the ADC is enabled
        // ADIE = ADC Interrupt Enable. When this bit is written to one and the I-bit in SREG is set, the ADC Conversion Complete Interrupt is activated.
        // ADPS  = These bits determine the division factor between the XTAL frequency and the input clock to the ADC.
        //
        ANALOG_ON;                      // #define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE)
        AdReady = 0;
        //------------------------------------------------------------

    if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;
        else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;
        else winkel_roll = Mess_IntegralRoll;

    if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;
        else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;
        else winkel_nick = Mess_IntegralNick;

        // Gier -----------------------------------------------------------------------------------------
        Mess_Integral_Gier += MesswertGier;
        ErsatzKompass += MesswertGier;
       
       
        // Kopplungsanteil  -----------------------------------------------------------------------------
    if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
    {
                tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
                tmpl3 *= Parameter_AchsKopplung2; //65
        tmpl3 /= 4096L;
        tmpl4 = (MesswertNick * winkel_roll) / 2048L;
        tmpl4 *= Parameter_AchsKopplung2; //65
        tmpl4 /= 4096L;
        KopplungsteilNickRoll = tmpl3;
        KopplungsteilRollNick = tmpl4;
        tmpl4 -= tmpl3;
        ErsatzKompass += tmpl4;
        if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen
               
        tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
        tmpl *= Parameter_AchsKopplung1;  // 90
        tmpl /= 4096L;
        tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
        tmpl2 *= Parameter_AchsKopplung1;
        tmpl2 /= 4096L;
        if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
        // MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
        }
        else  tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;

        TrimRoll = tmpl - tmpl2 / 100L;
        TrimNick = -tmpl2 + tmpl / 100L;

        // Kompasswert begrenzen -----------------------------------------------------------------------------------------------
        if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;                        // 360° Umschlag
        if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
       
        // Roll ------------------------------------------------------------
    Mess_IntegralRoll2 += MesswertRoll + TrimRoll;
    Mess_IntegralRoll +=  MesswertRoll + TrimRoll - 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;
    }
       
        // Nick -------------------------------------------------------------------
    Mess_IntegralNick2 += MesswertNick + TrimNick;
    Mess_IntegralNick  += MesswertNick + TrimNick - 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;
    }

    Integral_Gier  = Mess_Integral_Gier;
    IntegralNick = Mess_IntegralNick;
    IntegralRoll = Mess_IntegralRoll;
    IntegralNick2 = Mess_IntegralNick2;
    IntegralRoll2 = Mess_IntegralRoll2;

        #define D_LIMIT 128

        MesswertNick = HiResNick / 8;
        MesswertRoll = HiResRoll / 8;

        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 > 2000) MesswertNick = +1000;  if(AdWertNick > 2015) MesswertNick = +2000; }
       
        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 > 2000) MesswertRoll = +1000;  if(AdWertRoll > 2015) MesswertRoll = +2000;  }

        if(Parameter_Gyro_D)
        {
                d2Nick = HiResNick - oldNick;
                oldNick = (oldNick + HiResNick)/2;
               
                if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
                else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
               
                MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;
                d2Roll = HiResRoll - oldRoll;
                oldRoll = (oldRoll + HiResRoll)/2;
                if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
                else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
                MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
                HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);
                HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);
        }

        if(RohMesswertRoll > 0) TrimRoll  += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
        else                    TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
       
        if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
        else                    TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;

        if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
        {
                if(RohMesswertNick > 256)       MesswertNick += 1 * (RohMesswertNick - 256);
                else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);
                if(RohMesswertRoll > 256)       MesswertRoll += 1 * (RohMesswertRoll - 256);
                else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256);
        }

    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--;
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
}
// *** EOF: Mittelwert(void) ********************************************************************************************************



// **********************************************************************************************************************************
// Messwerte beim Ermitteln der Nullage
// ----------------------------------------
void CalibrierMittelwert(void)
{
    if(PlatinenVersion == 13) SucheGyroOffset();
        ANALOG_OFF;                                                                             // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
        MesswertNick = AdWertNick;
        MesswertRoll = AdWertRoll;
        MesswertGier = AdWertGier;
        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;                                                 // #define ACC_AMPLIFY    6
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
        Mittelwert_AccHoch = (long)AdWertAccHoch;
       
    // ADCSRA = Analog Digital Conversion Status Register A     -> ADEN ADSC ADATE ADIF ADIE ADPS2 ADPS1 ADPS0 
        // ---------------------------------------------------------------------------------------------------
        // ADEN  = Writing this bit to one enables the ADC
        // ADSC  = start conversion
        // ADATE = When this bit is written to one, Auto Triggering of the ADC is enabled
        // ADIE = ADC Interrupt Enable. When this bit is written to one and the I-bit in SREG is set, the ADC Conversion Complete Interrupt is activated.
        // ADPS  = These bits determine the division factor between the XTAL frequency and the input clock to the ADC.
        //
        ANALOG_ON;      // swich on ADC // #define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE)
   
        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--;
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;

        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
}
// *** EOF: CalibrierMittelwert() ***************************************************************************************************



// ***************************************************************************************************************************
// take over the motor values from Motor[] and start I2C-Bus
// ----------------------------------------------------------
void SendMotorData(void)
{
        unsigned char i;
       
        if(!MotorenEin)
        {
                MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
               
                for(i=0; i < MAX_MOTORS; i++)                                           // #define MAX_MOTORS      4
                {
                        if(!PC_MotortestActive)  MotorTest[i] = 0;
                        Motor[i] = MotorTest[i];
                }
               
                if(PC_MotortestActive) PC_MotortestActive--;
        }
        else MikroKopterFlags |= FLAG_MOTOR_RUN;                                // #define FLAG_MOTOR_RUN  1
       
        DebugOut.Analog[12] = Motor[0];                                         // see also fc.c line 1270
        DebugOut.Analog[13] = Motor[1];
       
        twi_state = 0;                                                                                  // it is assumed that all Motor[] have ther new value now -> see line 1645
        motor = 0;
        i2c_start();                                                                                    // start I2C Interrupt Mode
}
// ***************************************************************************************************************************



// ********************************************************************************************************************************
// Trägt gegebenfalls das Poti als Parameter ein
//----------------------------------------------
void ParameterZuordnung(void)
{
        #define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
        #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; }
       
        CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
        CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
        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_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z,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_Gyro_D,EE_Parameter.Gyro_D,0,255);
        CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P,10,255);
        CHK_POTI(Parameter_Gyro_Gier_I,EE_Parameter.Gyro_Gier_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_ServoRollControl,EE_Parameter.ServoRollControl,0,255);
        CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
        CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
        CHK_POTI(Parameter_AchsKopplung2,    EE_Parameter.AchsKopplung2,0,255);
        CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255);
        CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
        CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
        CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
        CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);
        Ki = 10300 / (Parameter_I_Faktor + 1);
        MAX_GAS = EE_Parameter.Gas_Max;                                                                                         // bei Beginner3 = 230
        MIN_GAS = EE_Parameter.Gas_Min;                                                                                         // bei Beginner3 = 8
}
//-----------------------------------------------------------------------------------------------------------

// ------------------------------------------only for balance ------------------------------------

unsigned char ucflg1=0x01, ucflg2=0x01, ucflg3=0x01;
int ipk[3] = {0,0,0};

int angle, desiredAngle, motorOutFront, motorOutRear;
int thrust;
int kp=0, kd=0, kdd=0;

int controllerP = 0, maxcontrollerP = 0, mincontrollerP = 0;
int controllerD = 0, maxcontrollerD = 0 , mincontrollerD = 0;
int controllerDD = 0, maxcontrollerDD = 0, mincontrollerDD = 0;        

int     gyroScaledOld = 0;
int gyroScaled = 0;

int filtersum = 0;
int filterDD = 0;

// EOF:  for balance ----------------------------------------------------------------------------


// **********************************************************************************************************************
// automatic control function - called from main.c
// ------------------------------------------------
void MotorRegler(void)
{
        int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
        int GierMischanteil,GasMischanteil;
        static long SummeNick=0,SummeRoll=0;
        static long sollGier = 0,tmp_long,tmp_long2;
       
        static long IntegralFehlerNick = 0;
        static long IntegralFehlerRoll = 0;
       
        static unsigned int RcLostTimer;
        static unsigned char delay_neutral = 0;                                                         //
        static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
        static char TimerWerteausgabe = 0;
        static char NeueKompassRichtungMerken = 0;
        static long ausgleichNick, ausgleichRoll;
       
        int IntegralNickMalFaktor,IntegralRollMalFaktor;
        // unsigned char i;
       
       
       
        if(--LoadHandler == 0) LoadHandler = 5;                                                 // verteilt die Prozessorlast
        Mittelwert();                                                                                                   // see fc.c line 273
        GRN_ON;                                                                                                                 // switch on green LED
       
        // -------------------------------------------------------------------
        // find out gas value
        // -------------------------------------------------------------------
        GasMischanteil = StickGas;
        if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
       
        // ----------------------------------------------------------------------------------
        // radio control receiving is bad
        // ----------------------------------------------------------------------------------
        if(SenderOkay < 100)
        {
        if(RcLostTimer) RcLostTimer--;
        else
        {
                        MotorenEin = 0;
                        MikroKopterFlags &= ~FLAG_NOTLANDUNG;
        }
       
                ROT_ON;
       
                if(modell_fliegt > 1000)  // wahrscheinlich in der Luft --> langsam absenken
        {
                        GasMischanteil = EE_Parameter.NotGas;
            MikroKopterFlags |= FLAG_NOTLANDUNG;
            PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;           // #define K_NICK    0
            PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;           // #define K_ROLL    1
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;             // #define K_GAS     2
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;             // #define K_GIER    3
        }
        else MotorenEin = 0;
        } // EOF : receiving radio control is bad
    else
        // -----------------------------------------------------------------------------------
        // radio control receiving is fine
        // -----------------------------------------------------------------------------------
    if(SenderOkay > 140)
    {
                MikroKopterFlags &= ~FLAG_NOTLANDUNG;
        RcLostTimer = EE_Parameter.NotGasZeit * 50;
       
                if(GasMischanteil > 40 && MotorenEin)
        {
            if(modell_fliegt < 0xffff) modell_fliegt++;
        }
       
                if((modell_fliegt < 256))
        {
                        SummeNick = 0;
            SummeRoll = 0;
            if(modell_fliegt == 250)
            {
                NeueKompassRichtungMerken = 1;
                sollGier = 0;
                                Mess_Integral_Gier = 0;
            }
        }
                else MikroKopterFlags |= FLAG_FLY;
               
        if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
        {
                        //----------------------------------------------------------------------------
                        // calibrate to zero
                        //----------------------------------------------------------------------------
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)     // neutral values
            {
                if(++delay_neutral > 200)                                                       // during the first 200 ms
                                {
                    GRN_OFF;
                    MotorenEin = 0;                                                                     // switch or keep motor in status "off"
                    delay_neutral = 0;
                    modell_fliegt = 0;
                                        //----------------------------------------------------------------------------------------------------------------
                                        // Bei ausgeschalteten Motoren Settings per Sender einstellen:
                                        //
                                        // Gas/Gier-Knüppel oben-links und gleichzeitig...
                                        //
                                        // Nick/Roll-Knüppel -> Links Mitte  = Setting 1; Sport, sehr agil
                                        // Nick/Roll-Knüppel -> Links Oben   = Setting 2; Normal
                                        // Nick/Roll-Knüppel -> Mitte Oben   = Setting 3; Beginner, empfohlen für erste Versuche
                                        // Nick/Roll-Knüppel -> Rechts Oben  = Setting 4
                                        // Nick/Roll-Knüppel -> Rechts Mitte = Setting 5
                                        //
                                        // Setting 1 = Gas rauf und Gier links dann Roll links  und Nick mitte
                                        // Setting 2 = Gas rauf und Gier links dann Roll links  und Nick rauf  
                                        // Setting 3 = Gas rauf und Gier links dann Roll mitte  und Nick rauf
                                        // Setting 4 = Gas rauf und Gier links dann Roll rechts und Nick rauf
                                        // Setting 5 = Gas rauf und Gier links dann Roll rechts und Nick mitte
                                        //----------------------------------------------------------------------------------------------------------------
                   
                    if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_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[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_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[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;
                        SetActiveParamSetNumber(setting);  // aktiven Datensatz merken
                    }
                                       
                                        if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
                    {
                        WinkelOut.CalcState = 1;                                // Kompass wird kalibriert
                        beeptime = 1000;
                    }
                    else
                    {
                                                //-------------------------------------------------------------------------------------------------------------------
                                                // Hier werden die Werte aus dem EEPROM entsprechend Setting eingelesen
                                                //-------------------------------------------------------------------------------------------------------------------
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); // #define  STRUCT_PARAM_LAENGE  sizeof(EE_Parameter)
                       
                                                if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))                                                    // Höhenregelung aktiviert?
                        {
                            if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
                        }
                                               
                                                // Vor jedem Starten des MK sollten die Gyros neu kalibriert werden.
                                                // Dazu wird der Gas/Gier-Knüppel einige Zeit in die obere linke Ecke gedrückt
                                                ServoActive = 0;
                        SetNeutral();
                                                ServoActive = 1;
                                                DDRD  |=0x80; // enable J7 -> Servo signal
                        Piep(GetActiveParamSetNumber(),120);
                    }
                } // Ende von "nicht sofort, sondern erst nach 200 ms"
            }
            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_NICK],0xff);          // Werte löschen
                    MotorenEin = 0;
                    delay_neutral = 0;
                    modell_fliegt = 0;
                    SetNeutral();
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
                    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(),120);
                }
            }
            else delay_neutral = 0;
        } //Ende Gas ist oben und Motoren aus
               
                //---------------------------------------------------------------------------------------------------------------------
                // gas is at bottom for switing on or off
                //---------------------------------------------------------------------------------------------------------------------
        if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < -85)                     // Gas unten
        {
            if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)                // Starten - Gierknüppel unten ganz rechts
            {
                                // -------------------------------------------------------------------------------------
                                // switch on motor
                                // -------------------------------------------------------------------------------------
                                if(++delay_einschalten > 200)
                {
                    delay_einschalten = 200;
                    modell_fliegt = 1;
                    MotorenEin = 1;
                    sollGier = 0;
                    Mess_Integral_Gier = 0;
                    Mess_Integral_Gier2 = 0;
                    Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
                    Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
                    Mess_IntegralNick2 = IntegralNick;
                    Mess_IntegralRoll2 = IntegralRoll;
                    SummeNick = 0;
                    SummeRoll = 0;
                    MikroKopterFlags |= FLAG_START;
                }
            }
            else delay_einschalten = 0;                                                                 // reset delay
                       
                        //----------------------------------------------------------------------------------------------
                        // switch off
                        //----------------------------------------------------------------------------------------------
                        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;
                       
        } // EOF : gas is at bottom
               
    } // EOF : receiving radio control is good

        //-------------------------------------------------------------------------------------------------------------------
        // neue Werte von der Funke
        //-------------------------------------------------------------------------------------------------------------------
       
        if(!NewPpmData-- || (MikroKopterFlags & FLAG_NOTLANDUNG))
        {
                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;
               
                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;
               
                StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
                if(StickGier > 2) StickGier -= 2;       else
                if(StickGier < -2) StickGier += 2; else StickGier = 0;
               
                StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
               
                GyroFaktor     = (Parameter_Gyro_P + 10.0);
                IntegralFaktor = Parameter_Gyro_I;
                GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);
                IntegralFaktorGier = Parameter_Gyro_Gier_I;
               
                //------------------------------------------------------------------------------------------
                // Analoge Steuerung nicht per Fersteuerung, sondern per serieller Schnittstelle
                //------------------------------------------------------------------------------------------
                if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128)
                {
                        StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
                        StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
                        StickGier += ExternControl.Gier;
                        ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
                        if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
                }
                if(StickGas < 0) StickGas = 0;
               
                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(MikroKopterFlags & FLAG_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;
               
        } // EOF : "neue Funksteuerungswerte"

        if(Looping_Roll || Looping_Nick)
        {
                if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
                TrichterFlug = 1;
        }

        // ---------------------------------------------------------------------------------------------
        // Bei Empfangsausfall im Flug Notlandung einleiten
        // ---------------------------------------------------------------------------------------------
        if(MikroKopterFlags & FLAG_NOTLANDUNG)
    {
                StickGier = 0;
                StickNick = 0;
                StickRoll = 0;
                GyroFaktor     = 90;
                IntegralFaktor = 120;
                GyroFaktorGier     = 90;
                IntegralFaktorGier = 120;
                Looping_Roll = 0;
                Looping_Nick = 0;
    } // ------------------- Ende : Notlandung -----------------------------------------------------


        //---------------------------------------------------------------------------------------------
        // Integrale auf ACC-Signal abgleichen
        //---------------------------------------------------------------------------------------------
        #define ABGLEICH_ANZAHL 256L

        MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
        MittelIntegralRoll  += IntegralRoll;
        MittelIntegralNick2 += IntegralNick2;
        MittelIntegralRoll2 += IntegralRoll2;

        if(Looping_Nick || Looping_Roll)
        {
                IntegralAccNick = 0;
                IntegralAccRoll = 0;
                MittelIntegralNick = 0;
                MittelIntegralRoll = 0;
                MittelIntegralNick2 = 0;
                MittelIntegralRoll2 = 0;
                Mess_IntegralNick2 = Mess_IntegralNick;
                Mess_IntegralRoll2 = Mess_IntegralRoll;
                ZaehlMessungen = 0;                                                     // counts one up as all ADC cases have been passed through -> see analog.c line 288
                LageKorrekturNick = 0;
                LageKorrekturRoll = 0;
        }

        //------------------------------------------------------------------------------------------------
        if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin))
        {
                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 > 64) || (MaxStickRoll > 64))
                {
                        tmp_long  /= 3;
                        tmp_long2 /= 3;
                }
               
                if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_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;
                if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
                if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
               
                //if(Poti2 > 20) { tmp_long = 0; tmp_long2 = 0;}
                Mess_IntegralNick -= tmp_long;
                Mess_IntegralRoll -= tmp_long2;
        } // --------------------------------- EOF: Looping -----------------------------------


        // -------------------------------------------------------------------------------------------------
        // nach 256 ADC Durchläufen wird abgeglichen
        // -------------------------------------------------------------------------------------------------   
        if(ZaehlMessungen >= ABGLEICH_ANZAHL)                                   // #define ABGLEICH_ANZAHL 256L
        {                                                                                                               // counts one up as all ADC cases have been passed through -> see analog.c line 288
                static int cnt = 0;
                static char last_n_p,last_n_n,last_r_p,last_r_n;
                static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
               
                if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp)
                {
                        MittelIntegralNick  /= ABGLEICH_ANZAHL;                 // #define ABGLEICH_ANZAHL 256L
                        MittelIntegralRoll  /= ABGLEICH_ANZAHL;
                       
                        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
                        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
                        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;    // #define ABGLEICH_ANZAHL 256L
                        LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
                       
                        if( (MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) )
                        {
                                LageKorrekturNick /= 2;
                                LageKorrekturRoll /= 2;
                        }
                       
                        //----------------------------------------------------
                        // Gyro-Drift ermitteln
                        //----------------------------------------------------
                        MittelIntegralNick2 /= ABGLEICH_ANZAHL;         // #define ABGLEICH_ANZAHL 256L
                        MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
                        tmp_long  = IntegralNick2 - IntegralNick;
                        tmp_long2 = IntegralRoll2 - IntegralRoll;
                        //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
                       
                        IntegralFehlerNick = tmp_long;
                        IntegralFehlerRoll = tmp_long2;
                        Mess_IntegralNick2 -= IntegralFehlerNick;
                        Mess_IntegralRoll2 -= IntegralFehlerRoll;
                       
                        if(EE_Parameter.Driftkomp)
                        {
                                if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }        // #define ABGLEICH_ANZAHL 256L
                                if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
                        }
                        GierGyroFehler = 0;
                       
                        #define FEHLER_LIMIT  (ABGLEICH_ANZAHL / 2)                     // #define ABGLEICH_ANZAHL 256L
                        #define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2)             //4
                        #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)            //16
                        #define BEWEGUNGS_LIMIT 20000
                       
                        // --------------- Nick ------------------------------------------------------------------
                        cnt = 1;                                                                                                                                // + labs(IntegralFehlerNick) / 4096;
                        if(labs(IntegralFehlerNick) > FEHLER_LIMIT1) cnt = 4;
                       
                        if( labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT )
                        {
                                if(IntegralFehlerNick >  FEHLER_LIMIT2)
                                {
                                        if(last_n_p)
                                        {
                                                cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
                                                ausgleichNick = IntegralFehlerNick / 8;
                                                if(ausgleichNick > 5000) ausgleichNick = 5000;
                                                LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;           // #define ABGLEICH_ANZAHL 256L
                                        }
                                        else last_n_p = 1;
                                }
                                else  last_n_p = 0;
                               
                                if(IntegralFehlerNick < -FEHLER_LIMIT2)
                                {
                                        if(last_n_n)
                                        {
                                                cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
                                                ausgleichNick = IntegralFehlerNick / 8;
                                                if(ausgleichNick < -5000) ausgleichNick = -5000;
                                                LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;           // #define ABGLEICH_ANZAHL 256L
                                        }
                                        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(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
                        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
                       
                        // ------------------ Roll --------------------------------------------------------------------
                        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
                       
                        if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4;
                       
                        ausgleichRoll = 0;
                       
                        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT )
                        {
                                if(IntegralFehlerRoll >  FEHLER_LIMIT2)
                                {
                                        if(last_r_p)
                                        {
                                                cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
                                                ausgleichRoll = IntegralFehlerRoll / 8;
                                                if(ausgleichRoll > 5000) ausgleichRoll = 5000;
                                                LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;   // #define ABGLEICH_ANZAHL 256L
                                        }
                                        else last_r_p = 1;
                                } else  last_r_p = 0;
                                if(IntegralFehlerRoll < -FEHLER_LIMIT2)
                                {
                                        if(last_r_n)
                                        {
                                                cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
                                                ausgleichRoll = IntegralFehlerRoll / 8;
                                                if(ausgleichRoll < -5000) ausgleichRoll = -5000;
                                                LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;   // #define ABGLEICH_ANZAHL 256L
                                        }
                                        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(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
                        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
                }
                else
                {
                        LageKorrekturRoll = 0;
                        LageKorrekturNick = 0;
                        TrichterFlug = 0;
                }
               
                if(!IntegralFaktor)             // z.B. bei Heading Hold wird nicht ausgeglichen
                {
                        LageKorrekturRoll = 0;
                        LageKorrekturNick = 0;
                }
               
                // ----------------------------------------------------------------------------------
                MittelIntegralNick_Alt = MittelIntegralNick;
                MittelIntegralRoll_Alt = MittelIntegralRoll;
                // ----------------------------------------------------------------------------------
                IntegralAccNick = 0;
                IntegralAccRoll = 0;
                IntegralAccZ = 0;
                MittelIntegralNick = 0;
                MittelIntegralRoll = 0;
                MittelIntegralNick2 = 0;
                MittelIntegralRoll2 = 0;
                ZaehlMessungen = 0;                             // counts one up as all ADC cases have been passed through -> see analog.c line 288
               
        }
        // EOF: if(ZaehlMessungen >= ABGLEICH_ANZAHL)


        // -------------------------------------------------------------------------------------------------------------
        // -------- Gieren -----------
        //
        // Für eine Drehung im Uhrzeigersinn wird die Drehzahl des linken und des rechten Propellers erhöht,
        // während die des vorderen und hinteren gesenkt wird
        // -------------------------------------------------------------------------------------------------------------
        if(abs(StickGier) > 15)                                                                                                                                 // war 35
        {
                KompassSignalSchlecht = 1000;
               
                if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))                                                                      // Orientation 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))                                 // CFG_KOMPASS_AKTIV = 0x08
    {
                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; // long GIER_GRAD_FAKTOR = 1291;
               
                if(abs(MesswertGier) > 128)
            {
                        fehler = 0;
                }
               
                if(!KompassSignalSchlecht && w < 25)
        {
                        GierGyroFehler += fehler;
                        if(NeueKompassRichtungMerken)
                        {
                                ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;                // long GIER_GRAD_FAKTOR = 1291;
                                KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);   
                                NeueKompassRichtungMerken = 0;
                        }
        }
               
                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;                  // long GIER_GRAD_FAKTOR = 1291;
                                //                                                                                                                                                                              // 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
               
        } // EOF: Kompass
        //---------------------------------------------------------------------------------------------------------------------------------



        //----------------------------------------------------------------------------------------------------------------------------------
        //  Debugwerte zuordnen / die zugehörige Texte stehen unter -> uart.c Zeile 46
        //----------------------------------------------------------------------------------------------------------------------------------
        if(!TimerWerteausgabe--)
        {
                TimerWerteausgabe = 24;
               
                DebugOut.Analog[0] = (int) (IntegralNick / (EE_Parameter.GyroAccFaktor * 4));
                DebugOut.Analog[1] = Mittelwert_AccNick / 4;
                DebugOut.Analog[2] = maxcontrollerDD;
                DebugOut.Analog[3] = mincontrollerDD;
                DebugOut.Analog[4] = MesswertNick;
                DebugOut.Analog[5] = HiResNick;
                DebugOut.Analog[6] = AdWertAccNick;
                // siehe Zeile fc.c Zeile 1660
                DebugOut.Analog[8] = 0;
                DebugOut.Analog[9] = UBat;
                DebugOut.Analog[10] = SenderOkay;
                DebugOut.Analog[11] = controllerP;
                // siehe fc.c Zeile     476
                DebugOut.Analog[14] = controllerD;
                DebugOut.Analog[15] = controllerDD;
                DebugOut.Analog[16] = PPM_in[3];
                DebugOut.Analog[17] = ipk[0];
                DebugOut.Analog[18] = ipk[1];
                DebugOut.Analog[19] = ipk[2];
                DebugOut.Analog[20] = ucflg1;                   // 0,1,2
                DebugOut.Analog[21] = PPM_in[5];                // switch chanel 5
                DebugOut.Analog[22] = MesswertNick;
                DebugOut.Analog[23] = maxcontrollerD;
                DebugOut.Analog[24] = mincontrollerD;
                DebugOut.Analog[25] = AdWertNick;                                                                                      
                DebugOut.Analog[26] = maxcontrollerP;                                                                                   // = 727
                DebugOut.Analog[27] = mincontrollerP;                                                                                   // = 730
                DebugOut.Analog[28] = PPM_in[4];                // Gier
                DebugOut.Analog[29] = PPM_in[3];                // Nick
                DebugOut.Analog[30] = PPM_in[2];                // Roll
                DebugOut.Analog[31] = PPM_in[1];                // Gas
        }
       
        // -----------------------------------------------------------------------
        //  Drehgeschwindigkeit und Drehwinkel zu einem Istwert zusammenfassen
        // -----------------------------------------------------------------------
        if(TrichterFlug)  {SummeRoll = 0; SummeNick = 0;};
       
        if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;
        if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0;
       
        #define TRIM_MAX 200
        if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else  if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
        if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
       
        MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
        MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
        MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
       
        // Maximalwerte abfangen
        // #define MAX_SENSOR  (4096*STICK_GAIN)
        #define MAX_SENSOR  (4096*4)
        if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
        if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
        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
        // ------------------------------------------------------------------------------------------------------------------------
       
        if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
        GasMischanteil *= STICK_GAIN;
       
        // if height control is activated
        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick))  // Höhenregelung
        {
                //--------------------------------- definitions-------------------------------------------
                #define HOOVER_GAS_AVERAGE 4096L                // 4096 * 2ms = 8.2s averaging
                #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8ms averaging
                #define OPA_OFFSET_STEP 10
               
                //------------------------------- variables ----------------------------------------------
                int HCGas, HeightDeviation;
                static int HeightTrimming = 0;  // rate for change of height setpoint
                static int FilterHCGas = 0;
                static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
                static unsigned long HooverGasFilter = 0;
                static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
            int CosAttitude;    // for projection of hoover gas
               
                // ------------------------------- get the current hooverpoint ----------------------------------------------------------
                //    if(LoadHandler == 1)
                {
                        // DebugOut.Analog[21] = HooverGas;
                        // DebugOut.Analog[18] = VarioMeter;
                       
                        // Expand the measurement
                        // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
                        if(!BaroExpandActive)
                        {
                                if(MessLuftdruck > 920)
                                {   // increase offset
                                        if(OCR0A < (255 - OPA_OFFSET_STEP))
                                        {
                                                ExpandBaro -= 1;
                                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
                                                beeptime = 300;
                                                BaroExpandActive = 350;
                                        }
                                        else
                                        {
                                                BaroAtLowerLimit = 1;
                                        }
                                }
                                // measurement of air pressure close to lower limit and
                                else
                                if(MessLuftdruck < 100)
                                {   // decrease offset
                                        if(OCR0A > OPA_OFFSET_STEP)
                                        {
                                                ExpandBaro += 1;
                                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
                                                beeptime = 300;
                                                BaroExpandActive = 350;
                                        }
                                        else
                                        {
                                                BaroAtUpperLimit = 1;
                                        }
                                }
                                else
                                {
                                        BaroAtUpperLimit = 0;
                                        BaroAtLowerLimit = 0;
                                }
                        }
                        else // delay, because of expanding the Baro-Range
                        {
                                // now clear the D-values
                                SummenHoehe = HoehenWert * SM_FILTER;                           // #define SM_FILTER 16
                                VarioMeter = 0;
                                BaroExpandActive--;
                        }
                       
                        // if height control is activated by an rc channel
                        if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)                     // Regler wird über Schalter gesteuert
                        {                                                                                                                               // check if parameter is less than activation threshold
                                if(Parameter_MaxHoehe < 50)                     // for 3 or 2-state switch height control is disabled in lowest position
                                {                                                                       //height control not active
                                        if(!delay--)
                                        {
                                                HoehenReglerAktiv = 0; // disable height control
                                                SollHoehe = HoehenWert;  // update SetPoint with current reading
                                                delay = 1;
                                        }
                                }
                                else
                                {       //height control is activated
                                        HoehenReglerAktiv = 1; // enable height control
                                        delay = 200;
                                }
                        }
                        else // no switchable height control
                        {
                                SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung;
                                HoehenReglerAktiv = 1;
                        }
                       
                        // calculate cos of nick and roll angle used for projection of the vertical hoover gas
                        tmp_int  = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg
                        tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR);  // roll angle in deg
                        CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg                                                                                                                                                                                           // xxx
                        LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle  
                        CosAttitude = c_cos_8192(CosAttitude);  // cos of actual attitude
                        if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG))
                        {
                                #define HEIGHT_TRIM_UP          0x01
                                #define HEIGHT_TRIM_DOWN        0x02
                                static unsigned char HeightTrimmingFlag = 0x00;
                               
                                #define HEIGHT_CONTROL_STICKTHRESHOLD 15
                                // Holger original version
                                // start of height control algorithm
                                // the height control is only an attenuation of the actual gas stick.
                                // I.e. it will work only if the gas stick is higher than the hover gas
                                // and the hover height will be allways larger than height setpoint.
                                if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) || !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER))  // Regler wird über Schalter gesteuert)
                                {  // old version
                                        HCGas = GasMischanteil; // take current stick gas as neutral point for the height control
                                        HeightTrimming = 0;
                                }
                                else
                                {
                                        // alternative height control
                                        // PD-Control with respect to hoover point
                                        // the thrust loss out of horizontal attitude is compensated
                                        // the setpoint will be fine adjusted with the gas stick position
                                        if(MikroKopterFlags & FLAG_FLY) // trim setpoint only when flying
                                        {   // gas stick is above hoover point
                                                if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit)
                                                {
                                                        if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN)
                                                        {
                                                                HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN;
                                                                SollHoehe = HoehenWert; // update setpoint to current heigth
                                                        }
                                                        HeightTrimmingFlag |= HEIGHT_TRIM_UP;
                                                        HeightTrimming += abs(StickGas - (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD));
                                                } // gas stick is below hoover point
                                                else if(StickGas < (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit )
                                                {
                                                        if(HeightTrimmingFlag & HEIGHT_TRIM_UP)
                                                        {
                                                                HeightTrimmingFlag &= ~HEIGHT_TRIM_UP;
                                                                SollHoehe = HoehenWert; // update setpoint to current heigth
                                                        }
                                                        HeightTrimmingFlag |= HEIGHT_TRIM_DOWN;
                                                        HeightTrimming -= abs(StickGas - (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD));
                                                }
                                                else // Gas Stick in Hoover Range
                                                {
                                                        if(HeightTrimmingFlag & (HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN))
                                                        {
                                                                HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN);
                                                                HeightTrimming = 0;
                                                                SollHoehe = HoehenWert; // update setpoint to current height
                                                                if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
                                                        }
                                                }
                                                // Trim height set point
                                                if(abs(HeightTrimming) > 512)
                                                {
                                                        SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint
                                                        HeightTrimming = 0;
                                                        if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75;
                                                        //update hoover gas stick value when setpoint is shifted
                                                        if(!EE_Parameter.Hoehe_StickNeutralPoint)
                                                        {
                                                                StickGasHoover = HooverGas/STICK_GAIN; //rescale back to stick value
                                                                StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
                                                                if(StickGasHoover < 70) StickGasHoover = 70;
                                                                else if(StickGasHoover > 150) StickGasHoover = 150;
                                                        }
                                                }
                                               
                                                if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active
                                        } //if MikroKopterFlags & MKFLAG_FLY
                                        else
                                        {
                                                SollHoehe = HoehenWert - 400;
                                                if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;
                                                else StickGasHoover = 120;
                                        }
                                        HCGas = HooverGas;      // take hoover gas (neutral point)
                                }
                               
                                if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT))
                                {
                                        // ------------------------- P-Part ----------------------------
                                        HeightDeviation = (int)(HoehenWert - SollHoehe); // positive when too high
                                        tmp_int = (HeightDeviation * (int)Parameter_Hoehe_P) / 16; // p-part
                                        HCGas -= tmp_int;
                                       
                                        // ------------------------- D-Part 1: Vario Meter ----------------------------
                                        tmp_int = VarioMeter / 8;
                                        if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction
                                        if(tmp_int > 0) tmp_int = VarioMeter + (tmp_int * tmp_int) / 4;
                                        else            tmp_int = VarioMeter - (tmp_int * tmp_int) / 4;
                                        tmp_int = (Parameter_Luftdruck_D * (long)(tmp_int)) / 128L; // scale to d-gain parameter
                                        LIMIT_MIN_MAX(tmp_int, -127, 255);
                                        HCGas -= tmp_int;
                                       
                                        // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
                                        tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
                                        LIMIT_MIN_MAX(tmp_int, -127, 255);
                                        HCGas -= tmp_int;
                                       
                                        // --------- limit deviation from hoover point within the target region ---------------------------------
                                        if( (abs(HeightDeviation) < 150)  && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero
                                        {
                                                LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point
                                        }
                                       
                                        if(BaroExpandActive) HCGas = HooverGas;
                                       
                                        // strech control output by inverse attitude projection 1/cos
                                        // + 1/cos(angle)  ++++++++++++++++++++++++++
                                        tmp_long2 = (int32_t)HCGas;
                                        tmp_long2 *= 8192L;
                                        tmp_long2 /= CosAttitude;
                                        HCGas = (int16_t)tmp_long2;
                                       
                                        // update height control gas averaging
                                        FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE;    // #define HC_GAS_AVERAGE 4
                                       
                                        // limit height control gas pd-control output
                                        LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN);
                                       
                                        // set GasMischanteil to HeightControlGasFilter
                                        if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)                                        // old version
                                        {  
                                                if(FilterHCGas > GasMischanteil) FilterHCGas = GasMischanteil; // nicht mehr als Gas
                                        }
                                       
                                        GasMischanteil = FilterHCGas;
                                }
                        }// EOF height control active
                        else // HC not active
                        {
                                //update hoover gas stick value when HC is not active
                                if(!EE_Parameter.Hoehe_StickNeutralPoint)
                                {
                                        StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
                                        StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
                                }
                                else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;
                                if(StickGasHoover < 70) StickGasHoover = 70;
                                else if(StickGasHoover > 150) StickGasHoover = 150;
                                FilterHCGas = GasMischanteil;
                        }
                       
                        // Hoover gas estimation by averaging gas control output on small z-velocities
                        // this is done only if height contol option is selected in global config and aircraft is flying
                        if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG))
                        {
                                if(HooverGasFilter == 0)  HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
                                if(abs(VarioMeter) < 100) // only on small vertical speed
                                {
                                        tmp_long2 = (int32_t)GasMischanteil; // take current thrust
                                        tmp_long2 *= CosAttitude;            // apply attitude projection
                                        tmp_long2 /= 8192;
                                       
                                        // average vertical projected thrust
                                        if(modell_fliegt < 2000) // the first 4 seconds
                                        {   // reduce the time constant of averaging by factor of 8 to get much faster a stable value
                                                HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L);
                                                HooverGasFilter += 8L * tmp_long2;
                                        }
                                        else if(modell_fliegt < 4000) // the first 8 seconds
                                        {   // reduce the time constant of averaging by factor of 4 to get much faster a stable value
                                                HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L);
                                                HooverGasFilter += 4L * tmp_long2;
                                        }
                                        else if(modell_fliegt < 8000) // the first 16 seconds
                                        {   // reduce the time constant of averaging by factor of 2 to get much faster a stable value
                                                HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L);
                                                HooverGasFilter += 2L * tmp_long2;
                                        }
                                        else //later
                                        {
                                                HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE;
                                                HooverGasFilter += tmp_long2;
                                        }
                                        HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE);
                                        if(EE_Parameter.Hoehe_HoverBand)
                                        {
                                                int16_t band;
                                                band = HooverGas / EE_Parameter.Hoehe_HoverBand; // the higher the parameter the smaller the range
                                                HooverGasMin = HooverGas - band;
                                                HooverGasMax = HooverGas + band;
                                        }
                                        else
                                        {       // no limit
                                                HooverGasMin = 0;
                                                HooverGasMax = 1023;
                                        }
                                }
                        }
                }
                // EOF:
       
        } // EOF: ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL

        // limit gas to parameter setting
        LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);
        if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
       
        // -----------------------------------------------------------------------------------------------
        // sind alle BL-Ctrl angeschlossen ? ist eventuell ein Motor defekt ?
        // -----------------------------------------------------------------------------------------------
        if(MissingMotor)                                                                                                        // see twimaster.c line 184
        if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
        {
                modell_fliegt = 1;
                GasMischanteil = MIN_GAS;
        }
       
        //-----------------------------------------------------------------------------------------------
        // Mischer und PI-Regler
        //-----------------------------------------------------------------------------------------------
       
       
        //-----------------------------------------------------------------------------------------------
        // Gier-Anteil (yaw)
        //-----------------------------------------------------------------------------------------------
        GierMischanteil = MesswertGier - sollGier * STICK_GAIN;                         // Regler für Gier
        #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 (pitch axis)
        // ------------------------------------------------------------------------------------------------------
        DiffNick = MesswertNick - StickNick;                                                                            // Differenz bestimmen
        if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick;              // I-Anteil bei Winkelregelung
        else  SummeNick += DiffNick;                                                                                            // I-Anteil bei Heading Hold
       
        if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
        if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
        pd_ergebnis_nick = DiffNick + SummeNick / Ki;                                                           // PI-Regler für Nick
       
        // Motor Vorn
        tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
        if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
        if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
       
        // ------------------------------------------------------------------------------------------------------
        // Roll-Achse (roll axis)
        // ------------------------------------------------------------------------------------------------------
        DiffRoll = MesswertRoll - StickRoll;                                                                            // Differenz bestimmen
        if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;                      // I-Anteil bei Winkelregelung
        else             SummeRoll += DiffRoll;                                                                         // I-Anteil bei Heading Hold
        if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
        if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
        pd_ergebnis_roll = DiffRoll + SummeRoll / Ki;                                                           // PI-Regler für Roll
       
        tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
        if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
        if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
       
       
       
        // -----------------------------------------------------------------------------------
        // kopterbalance
        // -----------------------------------------------------------------------------------
       
       
        //------------------------------------------------------------------------------------
        // set the parameters with radio control
        //------------------------------------------------------------------------------------
        if((PPM_in[2] < -60) && (ucflg2==0x01))
        {              
                ucflg2=0x00;
               
                if(ucflg1 == 0x02)
                {
                        ucflg1=0x00;
                }
                else ucflg1++;
        }

        if((PPM_in[2] > -20) && (PPM_in[2] < 20))
        {
                ucflg2=0x01;
        }
       
        // -----------------------------------------------------------------------------------
        // set analog value at according parameter
        // -----------------------------------------------------------------------------------

        if((PPM_in[5] < -80L) && (ucflg3==0x01))
        {
                ucflg3=0x00;
                if((ucflg1==0x00) || (ucflg1==0x01) || (ucflg1==0x02)) ipk[ucflg1] += PPM_in[3];
        }
       
        if(PPM_in[5] > 80L)
        {
                ucflg3=0x01;
        }
       
        kp =  ipk[0];
        kd =  ipk[1];
        kdd = ipk[2];

        // -------------------------------------------------------------------------------------
        // set trust with radio control
        // -----------------------------------------------------------------------------------
       
        thrust = PPM_in[1] + 127;                                       // set turnrate speed
       
        if(thrust<10)                                                           // limit min thrust
        {
                thrust = 10;
        }

        if(thrust>50)                                                           // limit max thrust
        {
                thrust = 50;
        }
       
        DebugOut.Analog[7] = thrust;

        // --------------------------------------------------------------------------------------
        desiredAngle = 0;
        angle = (int) (IntegralNick / (EE_Parameter.GyroAccFaktor * 4));
        gyroScaled = HiResNick;                                                                                                 // Gyro

        // --------------------------------------------------------------------------------------
        controllerP = (PPM_in[2]/4) + (kp * ((desiredAngle - angle)/16));               // proportional & manual control
       
        if(controllerP > maxcontrollerP) maxcontrollerP = controllerP;
        if(controllerP < mincontrollerP) mincontrollerP = controllerP;

        // --------------------------------------------------------------------------------------
        controllerD = (-gyroScaled * kd)/512;  

        if(controllerD > maxcontrollerD) maxcontrollerD = controllerD;
        if(controllerD < mincontrollerD) mincontrollerD = controllerD;

        // --------------------------------------------------------------------------------------
        controllerDD = (gyroScaledOld - gyroScaled) * kdd;
        filtersum = filtersum - filterDD + controllerDD;
        filterDD = filtersum/8;
        controllerDD = filterDD / 64;
        gyroScaledOld = gyroScaled;

        if(controllerDD > maxcontrollerDD) maxcontrollerDD = controllerDD;
        if(controllerDD < mincontrollerDD) mincontrollerDD = controllerDD;
       
        // --------------------------------------------------------------------------------------
        motorOutRear =  thrust + controllerD + controllerDD + controllerP;
        motorOutFront = thrust - controllerD - controllerDD - controllerP;  

        if(motorOutFront>50) motorOutFront = 50;
        if(motorOutFront<1) motorOutFront = 0;

        if(motorOutRear>50) motorOutRear = 50;
        if(motorOutRear<1) motorOutRear = 0;
       
        Motor[1] = motorOutRear;
        Motor[0] = motorOutFront;

}
// *** EOF: MotorRegler() ********************************************************************************************************