Subversion Repositories FlightCtrl

Rev

Rev 1538 | Blame | Compare with Previous | Last modification | View Log | RSS feed

/*#######################################################################################
Flight Control
#######################################################################################*/

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) Holger Buss, Ingo Busker
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <stdlib.h>
#include <avr/io.h>

#include "main.h"
#include "eeprom.h"
#include "timer0.h"
#include "analog.h"
#include "printf_P.h"
#include "fc.h"
#include "uart0.h"
#include "rc.h"
#include "twimaster.h"
#include "timer2.h"
#include "mymath.h"
#include "isqrt.h"
#ifdef USE_KILLAGREG
#include "mm3.h"
#include "gps.h"
#endif
#ifdef USE_MK3MAG
#include "mk3mag.h"
#include "gps.h"
#endif
#include "led.h"
#ifdef USE_NAVICTRL
#include "spi.h"
#endif


#define STICK_GAIN 4
#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;}

// gyro readings
int16_t GyroNick, GyroRoll, GyroYaw;

// gyro bias
int16_t BiasHiResGyroNick = 0, BiasHiResGyroRoll = 0, AdBiasGyroYaw = 0;

// accelerations
int16_t AccNick, AccRoll, AccTop;

// neutral acceleration readings
int16_t AdBiasAccNick = 0, AdBiasAccRoll = 0;
volatile float AdBiasAccTop = 0;
// the additive gyro rate corrections according to the axis coupling
int16_t TrimNick, TrimRoll;


// attitude gyro integrals
int32_t IntegralGyroNick = 0,IntegralGyroNick2 = 0;
int32_t IntegralGyroRoll = 0,IntegralGyroRoll2 = 0;
int32_t IntegralGyroYaw = 0;
int32_t ReadingIntegralGyroNick = 0, ReadingIntegralGyroNick2 = 0;
int32_t ReadingIntegralGyroRoll = 0, ReadingIntegralGyroRoll2 = 0;
int32_t ReadingIntegralGyroYaw = 0;
int32_t MeanIntegralGyroNick;
int32_t MeanIntegralGyroRoll;

// attitude acceleration integrals
int32_t MeanAccNick = 0, MeanAccRoll = 0;
volatile int32_t ReadingIntegralTop = 0;

// compass course
int16_t CompassHeading = -1; // negative angle indicates invalid data.
int16_t CompassCourse = -1;
int16_t CompassOffCourse = 0;
uint8_t CompassCalState = 0;
uint8_t FunnelCourse = 0;
uint16_t BadCompassHeading = 500;
int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass
int16_t YawGyroDrift;


int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;


// MK flags
uint16_t ModelIsFlying = 0;
uint8_t  volatile MKFlags = 0;

int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;

uint8_t GyroPFactor, GyroIFactor;       // the PD factors for the attitude control
uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control

int16_t Ki = 10300 / 33;

int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;


uint8_t RequiredMotors = 0;


// stick values derived by rc channels readings
int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0;
int16_t GPSStickNick = 0, GPSStickRoll = 0;

int16_t MaxStickNick = 0, MaxStickRoll = 0;

// stick values derived by uart inputs
int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20;

int32_t SetPointHeight = 0;

int16_t AttitudeCorrectionRoll = 0, AttitudeCorrectionNick = 0;

uint8_t LoopingNick = 0, LoopingRoll = 0;
uint8_t LoopingLeft = 0, LoopingRight = 0, LoopingDown = 0, LoopingTop = 0;


fc_param_t FCParam = {48,251,16,58,64,64,8,150,150,150,150,2,10,0,0,0,0,0,0,0,0,100,100,70,90,65,64,100,0,0,0};



/************************************************************************/
/*  Filter for motor value smoothing                                    */
/************************************************************************/
int16_t MotorSmoothing(int16_t newvalue, int16_t oldvalue)
{
        int16_t motor;
        if(newvalue > oldvalue) motor = (1 * (int16_t)oldvalue + newvalue) / 2;  //mean of old and new
        else                                    motor = newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
        return(motor);
}

/************************************************************************/
/*  Creates numbeeps beeps at the speaker                               */
/************************************************************************/
void Beep(uint8_t numbeeps, uint16_t duration)
{
        if(MKFlags & MKFLAG_MOTOR_RUN) return; // never with running motors!!!
        while(numbeeps--)
        {
                BeepTime = duration; // in ms second
                Delay_ms(duration * 2); // blocks 2 times beep duration,
                // this will block the flight control loop !!!!!
                // therefore do not use this function if motors are running
        }
}

/************************************************************************/
/*  Neutral Readings                                                    */
/************************************************************************/
void SetNeutral(uint8_t AccAdjustment)
{
        uint8_t i;
        int32_t Sum_1, Sum_2 = 0, Sum_3;

        //Servo_Off(); // disable servo output

        AdBiasAccNick = 0;
        AdBiasAccRoll = 0;
        AdBiasAccTop = 0;

    BiasHiResGyroNick = 0;
        BiasHiResGyroRoll = 0;
        AdBiasGyroYaw = 0;

    FCParam.AxisCoupling1 = 0;
    FCParam.AxisCoupling2 = 0;

    ExpandBaro = 0;

        // sample values with bias set to zero
    Delay_ms_Mess(100);

    if(BoardRelease == 13) SearchDacGyroOffset();

    if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR))  // air pressure sensor installed?
    {
                if((AdAirPressure > AIR_PRESSURE_SEARCH_MAX) || (AdAirPressure < AIR_PRESSURE_SEARCH_MIN)) SearchAirPressureOffset();
    }

    // determine gyro bias by averaging (require no rotation movement)
    #define GYRO_BIAS_AVERAGE 32
    Sum_1 = 0;
        Sum_2 = 0;
        Sum_3 = 0;
    for(i=0; i < GYRO_BIAS_AVERAGE; i++)
    {
                Delay_ms_Mess(10);
                Sum_1 += AdValueGyroNick * HIRES_GYRO_AMPLIFY;
                Sum_2 += AdValueGyroRoll * HIRES_GYRO_AMPLIFY;
                Sum_3 += AdValueGyroYaw;
        }
        BiasHiResGyroNick = (int16_t)((Sum_1 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE);
        BiasHiResGyroRoll = (int16_t)((Sum_2 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE);
        AdBiasGyroYaw     = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE);

    if(AccAdjustment != NO_ACC_CALIB)
    {
                // determine acc bias by averaging (require horizontal adjustment in nick and roll attitude)
                #define ACC_BIAS_AVERAGE 10
                Sum_1 = 0;
                Sum_2 = 0;
                Sum_3 = 0;
                for(i=0; i < ACC_BIAS_AVERAGE; i++)
                {
                        Delay_ms_Mess(10);
                        Sum_1 += AdValueAccNick;
                        Sum_2 += AdValueAccRoll;
                        Sum_3 += AdValueAccZ;
                }
                // use abs() to avoid negative bias settings because of adc sign flip in adc.c
                AdBiasAccNick = (int16_t)((abs(Sum_1) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE);
                AdBiasAccRoll = (int16_t)((abs(Sum_2) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE);
                AdBiasAccTop  = (int16_t)((abs(Sum_3) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE);

                // Save ACC neutral settings to eeprom
                SetParamWord(PID_ACC_NICK, (uint16_t)AdBiasAccNick);
                SetParamWord(PID_ACC_ROLL, (uint16_t)AdBiasAccRoll);
                SetParamWord(PID_ACC_TOP,  (uint16_t)AdBiasAccTop);
    }
    else // restore from eeprom
    {
                AdBiasAccNick = (int16_t)GetParamWord(PID_ACC_NICK);
            AdBiasAccRoll = (int16_t)GetParamWord(PID_ACC_ROLL);
            AdBiasAccTop  = (int16_t)GetParamWord(PID_ACC_TOP);

                if((AdBiasAccNick > 2048) || (AdBiasAccRoll > 2048) || (AdBiasAccTop > 1024))
                {
                        printf("\n\rACC not calibrated!\r\n");
                        AdBiasAccNick = 1024;
                        AdBiasAccRoll = 1024;
                        AdBiasAccTop = 725;
                }
    }
        // offset for height reading
    StartAirPressure = AirPressure;

    // setting acc bias values has an influence in the analog.c ISR
    // therefore run measurement for 100ms to achive stable readings
        Delay_ms_Mess(100);

        ReadingVario = 0;

    // reset acc averaging and integrals
    AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick;
    AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
    AccTop  = AdValueAccTop;
    ReadingIntegralTop = AdValueAccTop * 1024;

        // and gyro readings
        GyroNick = 0;
        GyroRoll = 0;
    GyroYaw = 0;

    // reset gyro integrals to acc guessing
    IntegralGyroNick = ParamSet.GyroAccFactor * (int32_t)AccNick;
    IntegralGyroRoll = ParamSet.GyroAccFactor * (int32_t)AccRoll;
        //ReadingIntegralGyroNick = IntegralGyroNick;
        //ReadingIntegralGyroRoll = IntegralGyroRoll;
    ReadingIntegralGyroNick2 = IntegralGyroNick;
    ReadingIntegralGyroRoll2 = IntegralGyroRoll;
    ReadingIntegralGyroYaw = 0;

        // update compass course to current heading
    CompassCourse = CompassHeading;
    // Inititialize YawGyroIntegral value with current compass heading
    YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
    YawGyroDrift = 0;

    BeepTime = 50;

        TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L;
        TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L;

    ExternHeightValue = 0;

    GPSStickNick = 0;
    GPSStickRoll = 0;

    MKFlags |= MKFLAG_CALIBRATE;

        FCParam.KalmanK = -1;
        FCParam.KalmanMaxDrift = 0;
        FCParam.KalmanMaxFusion = 32;

        Poti1 = PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_POTI_OFFSET;
        Poti2 = PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_POTI_OFFSET;
        Poti3 = PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_POTI_OFFSET;
        Poti4 = PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_POTI_OFFSET;

        //Servo_On(); //enable servo output
        RC_Quality = 100;
}

/************************************************************************/
/*  Averaging Measurement Readings                                      */
/************************************************************************/
void Mean(void)
{
    int32_t tmpl = 0, tmpl2 = 0, tmp13 = 0, tmp14 = 0;
    int16_t FilterGyroNick, FilterGyroRoll;
        static int16_t Last_GyroRoll = 0, Last_GyroNick = 0;
        int16_t d2Nick, d2Roll;
        int32_t AngleNick, AngleRoll;
        int16_t CouplingNickRoll = 0, CouplingRollNick = 0;

        // Get bias free gyro readings
        GyroNick = HiResGyroNick / HIRES_GYRO_AMPLIFY; // unfiltered gyro rate
    FilterGyroNick = FilterHiResGyroNick / HIRES_GYRO_AMPLIFY; // use filtered gyro rate

        // handle rotation rates that violate adc ranges
        if(AdValueGyroNick < 15)   GyroNick = -1000;
        if(AdValueGyroNick <  7)   GyroNick = -2000;
        if(BoardRelease == 10)
        {
                if(AdValueGyroNick > 1010) GyroNick = +1000;
                if(AdValueGyroNick > 1017) GyroNick = +2000;
        }
        else
        {
                if(AdValueGyroNick > 2000) GyroNick = +1000;
                if(AdValueGyroNick > 2015) GyroNick = +2000;
        }

        GyroRoll = HiResGyroRoll / HIRES_GYRO_AMPLIFY; // unfiltered gyro rate
        FilterGyroRoll = FilterHiResGyroRoll / HIRES_GYRO_AMPLIFY; // use filtered gyro rate
        // handle rotation rates that violate adc ranges
        if(AdValueGyroRoll < 15)   GyroRoll = -1000;
        if(AdValueGyroRoll <  7)   GyroRoll = -2000;
        if(BoardRelease == 10)
        {
                if(AdValueGyroRoll > 1010) GyroRoll = +1000;
                if(AdValueGyroRoll > 1017) GyroRoll = +2000;
        }
        else
        {
                if(AdValueGyroRoll > 2000) GyroRoll = +1000;
                if(AdValueGyroRoll > 2015) GyroRoll = +2000;
        }

        GyroYaw   = AdBiasGyroYaw - AdValueGyroYaw;

        // Acceleration Sensor
        // lowpass acc measurement and scale AccNick/AccRoll by a factor of ACC_AMPLIFY to have a better resolution
        AccNick  = ((int32_t)AccNick * 3L + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 4L;
        AccRoll  = ((int32_t)AccRoll * 3L + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 4L;
        AccTop   = ((int32_t)AccTop  * 3L + ((int32_t)AdValueAccTop)) / 4L;

        // sum acc sensor readings for later averaging
    MeanAccNick  += ACC_AMPLIFY * AdValueAccNick;
    MeanAccRoll  += ACC_AMPLIFY * AdValueAccRoll;

    NaviAccNick += AdValueAccNick;
    NaviAccRoll += AdValueAccRoll;
    NaviCntAcc++;


        // enable ADC to meassure next readings, before that point all variables should be read that are written by the ADC ISR
        ADC_Enable();
        ADReady = 0;

        // limit angle readings for axis coupling calculations
        #define ANGLE_LIMIT 93000L // aprox. 93000/GYRO_DEG_FACTOR = 82 deg

        AngleNick = ReadingIntegralGyroNick;
        LIMIT_MIN_MAX(AngleNick, -ANGLE_LIMIT, ANGLE_LIMIT);

        AngleRoll = ReadingIntegralGyroRoll;
        LIMIT_MIN_MAX(AngleRoll, -ANGLE_LIMIT, ANGLE_LIMIT);


        // Yaw
        // calculate yaw gyro integral (~ to rotation angle)
        YawGyroHeading += GyroYaw;
        ReadingIntegralGyroYaw  += GyroYaw;


        // Coupling fraction
        if(! LoopingNick && !LoopingRoll && (ParamSet.Config0 & CFG0_AXIS_COUPLING_ACTIVE))
        {
                tmp13 = (FilterGyroRoll * AngleNick) / 2048L;
                tmp13 *= FCParam.AxisCoupling2;
                tmp13 /= 4096L;
                CouplingNickRoll = tmp13;

                tmp14 = (FilterGyroNick * AngleRoll) / 2048L;
                tmp14 *= FCParam.AxisCoupling2;
                tmp14 /= 4096L;
                CouplingRollNick = tmp14;

                tmp14 -= tmp13;
                YawGyroHeading += tmp14;
                if(!FCParam.AxisCouplingYawCorrection)  ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw

                tmpl = ((GyroYaw + tmp14) * AngleNick) / 2048L;
                tmpl *= FCParam.AxisCoupling1;
                tmpl /= 4096L;

                tmpl2 = ((GyroYaw + tmp14) * AngleRoll) / 2048L;
                tmpl2 *= FCParam.AxisCoupling1;
                tmpl2 /= 4096L;
                if(abs(GyroYaw > 64))
                {
                        if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1;
                }

                TrimNick = -tmpl2 + tmpl / 100L;
                TrimRoll = tmpl - tmpl2 / 100L;
        }
        else
        {
                CouplingNickRoll = 0;
                CouplingRollNick = 0;
                TrimNick = 0;
                TrimRoll = 0;
        }


        // Yaw

    // limit YawGyroHeading proportional to 0° to 360°
    if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR))      YawGyroHeading -= 360L * GYRO_DEG_FACTOR;  // 360° Wrap
        if(YawGyroHeading < 0)                                          YawGyroHeading += 360L * GYRO_DEG_FACTOR;

        // Roll
        ReadingIntegralGyroRoll2 += FilterGyroRoll + TrimRoll;
        ReadingIntegralGyroRoll  += FilterGyroRoll + TrimRoll- AttitudeCorrectionRoll;
        if(ReadingIntegralGyroRoll > TurnOver180Roll)
        {
                ReadingIntegralGyroRoll  = -(TurnOver180Roll - 10000L);
                ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;
        }
        if(ReadingIntegralGyroRoll < -TurnOver180Roll)
        {
                ReadingIntegralGyroRoll =  (TurnOver180Roll - 10000L);
                ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;
        }

        // Nick
        ReadingIntegralGyroNick2 += FilterGyroNick + TrimNick;
        ReadingIntegralGyroNick  += FilterGyroNick + TrimNick - AttitudeCorrectionNick;
        if(ReadingIntegralGyroNick > TurnOver180Nick)
        {
                ReadingIntegralGyroNick = -(TurnOver180Nick - 25000L);
                ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
        }
        if(ReadingIntegralGyroNick < -TurnOver180Nick)
        {
                ReadingIntegralGyroNick =  (TurnOver180Nick - 25000L);
                ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
        }

    IntegralGyroYaw    = ReadingIntegralGyroYaw;
    IntegralGyroNick   = ReadingIntegralGyroNick;
    IntegralGyroRoll   = ReadingIntegralGyroRoll;
    IntegralGyroNick2  = ReadingIntegralGyroNick2;
    IntegralGyroRoll2  = ReadingIntegralGyroRoll2;


        #define D_LIMIT 128

        if(FCParam.GyroD)
        {
                d2Nick = (HiResGyroNick - Last_GyroNick); // change of gyro rate
                Last_GyroNick = (Last_GyroNick + HiResGyroNick) / 2;
                LIMIT_MIN_MAX(d2Nick, -D_LIMIT, D_LIMIT);
                GyroNick += (d2Nick * (int16_t)FCParam.GyroD) / 16;

                d2Roll = (HiResGyroRoll - Last_GyroRoll); // change of gyro rate
                Last_GyroRoll = (Last_GyroRoll + HiResGyroRoll) / 2;
                LIMIT_MIN_MAX(d2Roll, -D_LIMIT, D_LIMIT);
                GyroRoll += (d2Roll * (int16_t)FCParam.GyroD) / 16;

                HiResGyroNick += (d2Nick * (int16_t)FCParam.GyroD);
                HiResGyroRoll += (d2Roll * (int16_t)FCParam.GyroD);
        }

        // Increase the roll/nick rate virtually proportional to the coupling to suppress a faster rotation
        if(FilterGyroNick > 0)  TrimNick += ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L;
        else                    TrimNick -= ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L;
        if(FilterGyroRoll > 0)  TrimRoll += ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L;
        else                    TrimRoll -= ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L;

        // increase the nick/roll rates virtually from the threshold of 245 to slow down higher rotation rates
        if((ParamSet.Config0 & CFG0_ROTARY_RATE_LIMITER) && ! LoopingNick && !LoopingRoll)
        {
                if(FilterGyroNick > 256)                GyroNick += 1 * (FilterGyroNick - 256);
                else if(FilterGyroNick < -256)  GyroNick += 1 * (FilterGyroNick + 256);
                if(FilterGyroRoll > 256)        GyroRoll += 1 * (FilterGyroRoll - 256);
                else if(FilterGyroRoll < -256)  GyroRoll += 1 * (FilterGyroRoll + 256);
        }

}


/************************************************************************/
/*  Transmit Motor Data via I2C                                         */
/************************************************************************/
void SendMotorData(void)
{
        uint8_t i;
    if(!(MKFlags & MKFLAG_MOTOR_RUN))
    {
                MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
                for(i = 0; i < MAX_MOTORS; i++)
                {
                        if(!MotorTest_Active) Motor[i].SetPoint = 0;
                        else                  Motor[i].SetPoint = MotorTest[i];
                }
                if(MotorTest_Active) MotorTest_Active--;
        }

        DebugOut.Analog[12] = Motor[0].SetPoint; // Front
        DebugOut.Analog[13] = Motor[1].SetPoint; // Rear
        DebugOut.Analog[14] = Motor[3].SetPoint; // Left
        DebugOut.Analog[15] = Motor[2].SetPoint; // Right
    //Start I2C Interrupt Mode
    I2C_Start(TWI_STATE_MOTOR_TX);
}


/************************************************************************/
/*  Map the parameter to poti values                                    */
/************************************************************************/
void ParameterMapping(void)
{
        if(RC_Quality > 160) // do the mapping of RC-Potis only if the rc-signal is ok
        // else the last updated values are used
        {
                 //update poti values by rc-signals
                #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) { 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(FCParam.MaxHeight,ParamSet.MaxHeight);
                CHK_POTI_MM(FCParam.HeightD,ParamSet.HeightD,0,100);
                CHK_POTI_MM(FCParam.HeightP,ParamSet.HeightP,0,100);
                CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect);
                CHK_POTI(FCParam.Height_GPS_Z,ParamSet.Height_GPS_Z);
                CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect);
                CHK_POTI_MM(FCParam.GyroP,ParamSet.GyroP,10,255);
                CHK_POTI(FCParam.GyroI,ParamSet.GyroI);
                CHK_POTI(FCParam.GyroD,ParamSet.GyroD);
                CHK_POTI_MM(FCParam.GyroYawP,ParamSet.GyroYawP,10,255);
                CHK_POTI(FCParam.GyroYawI,ParamSet.GyroYawI);
                CHK_POTI(FCParam.IFactor,ParamSet.IFactor);
                CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1);
                CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2);
                CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3);
                CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4);
                CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5);
                CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6);
                CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7);
                CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8);
                CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl);
                CHK_POTI(FCParam.ServoRollControl,ParamSet.ServoRollControl);
                CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit);
                CHK_POTI(FCParam.AxisCoupling1,ParamSet.AxisCoupling1);
                CHK_POTI(FCParam.AxisCoupling2,ParamSet.AxisCoupling2);
                CHK_POTI(FCParam.AxisCouplingYawCorrection,ParamSet.AxisCouplingYawCorrection);
                CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability);
                CHK_POTI_MM(FCParam.J16Timing,ParamSet.J16Timing,1,255);
                CHK_POTI_MM(FCParam.J17Timing,ParamSet.J17Timing,1,255);
                #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
                CHK_POTI(FCParam.NaviGpsModeControl,ParamSet.NaviGpsModeControl);
                CHK_POTI(FCParam.NaviGpsGain,ParamSet.NaviGpsGain);
                CHK_POTI(FCParam.NaviGpsP,ParamSet.NaviGpsP);
                CHK_POTI(FCParam.NaviGpsI,ParamSet.NaviGpsI);
                CHK_POTI(FCParam.NaviGpsD,ParamSet.NaviGpsD);
                CHK_POTI(FCParam.NaviGpsACC,ParamSet.NaviGpsACC);
                CHK_POTI_MM(FCParam.NaviOperatingRadius,ParamSet.NaviOperatingRadius,10, 255);
                CHK_POTI(FCParam.NaviWindCorrection,ParamSet.NaviWindCorrection);
                CHK_POTI(FCParam.NaviSpeedCompensation,ParamSet.NaviSpeedCompensation);
                #endif
                CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl);
                Ki = 10300 / ( FCParam.IFactor + 1 );
        }
}


void SetCompassCalState(void)
{
        static uint8_t stick = 1;

    // if nick is centered or top set stick to zero
        if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -20) stick = 0;
        // if nick is down trigger to next cal state
        if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick)
        {
                stick = 1;
                CompassCalState++;
                if(CompassCalState < 5) Beep(CompassCalState, 150);
                else BeepTime = 1000;
        }
}



/************************************************************************/
/*  MotorControl                                                        */
/************************************************************************/
void MotorControl(void)
{
        int16_t tmp_int1, tmp_int2;
        int32_t tmp_long, tmp_long2;

        // Mixer Fractions that are combined for Motor Control
        int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;

        // PID controller variables
        int16_t DiffNick, DiffRoll;
        int16_t PDPartNick, PDPartRoll, PDPartYaw, PPartNick, PPartRoll;
        static int32_t IPartNick = 0, IPartRoll = 0;

        static int32_t SetPointYaw = 0;
        static int32_t IntegralGyroNickError = 0, IntegralGyroRollError = 0;
        static int32_t CorrectionNick, CorrectionRoll;
        static uint16_t RcLostTimer;
        static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
        static int8_t  TimerDebugOut = 0;
        static uint16_t UpdateCompassCourse = 0;
        // high resolution motor values for smoothing of PID motor outputs
        static int16_t MotorValue[MAX_MOTORS];
        uint8_t i;

        Mean();
        GRN_ON;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// RC-signal is bad
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(RC_Quality < 100)  // the rc-frame signal is not reveived or noisy
        {
                if(RcLostTimer) RcLostTimer--; // decremtent timer after rc sigal lost
                else // rc lost countdown finished
                {
                        MKFlags &= ~(MKFLAG_MOTOR_RUN|MKFLAG_EMERGENCY_LANDING); // clear motor run flag that stop the motors in SendMotorData()
                }
                RED_ON; // set red led
                if(ModelIsFlying > 1000)  // wahrscheinlich in der Luft --> langsam absenken
                {
                        MKFlags |= (MKFLAG_EMERGENCY_LANDING); // set flag for emergency landing
                        // set neutral rc inputs
                        PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] = 0;
                        PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
                        PPM_diff[ParamSet.ChannelAssignment[CH_YAW]] = 0;
                        PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0;
                        PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
                        PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0;
                }
                else MKFlags &= ~(MKFLAG_MOTOR_RUN); // clear motor run flag that stop the motors in SendMotorData()
        } // eof RC_Quality < 100
        else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// RC-signal is good
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(RC_Quality > 140)
        {
                MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
                // reset emergency timer
                RcLostTimer = ParamSet.EmergencyGasDuration * 50;
                #define GAS_FLIGHT_THRESHOLD 40
                if(StickGas > GAS_FLIGHT_THRESHOLD && (MKFlags & MKFLAG_MOTOR_RUN) )
                {
                        if(ModelIsFlying < 0xFFFF) ModelIsFlying++;
                }
                if(ModelIsFlying < 256)
                {
                        IPartNick = 0;
                        IPartRoll = 0;
                        StickYaw = 0;
                        if(ModelIsFlying == 250)
                        {
                                UpdateCompassCourse = 1;
                                ReadingIntegralGyroYaw = 0;
                                SetPointYaw = 0;
                        }
                }
                else MKFlags |= MKFLAG_FLY; // set fly flag

                if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_POTI_OFFSET) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_POTI_OFFSET && Poti1) Poti1--;
                if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_POTI_OFFSET) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_POTI_OFFSET && Poti2) Poti2--;
                if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_POTI_OFFSET) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_POTI_OFFSET && Poti3) Poti3--;
                if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_POTI_OFFSET) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_POTI_OFFSET && Poti4) Poti4--;
                //PPM24-Extension
                if(Poti5 < PPM_in[9]  + RC_POTI_OFFSET) Poti5++; else if(Poti5 > PPM_in[9]  + RC_POTI_OFFSET && Poti5) Poti5--;
                if(Poti6 < PPM_in[10] + RC_POTI_OFFSET) Poti6++; else if(Poti6 > PPM_in[10] + RC_POTI_OFFSET && Poti6) Poti6--;
                if(Poti7 < PPM_in[11] + RC_POTI_OFFSET) Poti7++; else if(Poti7 > PPM_in[11] + RC_POTI_OFFSET && Poti7) Poti7--;
                if(Poti8 < PPM_in[12] + RC_POTI_OFFSET) Poti8++; else if(Poti8 > PPM_in[12] + RC_POTI_OFFSET && Poti8) Poti8--;
                //limit poti values
                #define POTI_MIN 0
                #define POTI_MAX 255
                LIMIT_MIN_MAX(Poti1, POTI_MIN, POTI_MAX);
                LIMIT_MIN_MAX(Poti2, POTI_MIN, POTI_MAX);
                LIMIT_MIN_MAX(Poti3, POTI_MIN, POTI_MAX);
                LIMIT_MIN_MAX(Poti4, POTI_MIN, POTI_MAX);
                //PPM24-Extension
                LIMIT_MIN_MAX(Poti5, POTI_MIN, POTI_MAX);
                LIMIT_MIN_MAX(Poti6, POTI_MIN, POTI_MAX);
                LIMIT_MIN_MAX(Poti7, POTI_MIN, POTI_MAX);
                LIMIT_MIN_MAX(Poti8, POTI_MIN, POTI_MAX);

                // if motors are off and the gas stick is in the upper position
                if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && !(MKFlags & MKFLAG_MOTOR_RUN) )
                {
                        // and if the yaw stick is in the leftmost position
                        if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// calibrate the neutral readings of all attitude sensors
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                        {
                                // gas/yaw joystick is top left
                                //  _________
                                // |x        |
                                // |         |
                                // |         |
                                // |         |
                                // |         |
                                //  ¯¯¯¯¯¯¯¯¯
                                if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
                                {
                                        delay_neutral = 0;
                                        GRN_OFF;
                                        ModelIsFlying = 0;
                                        // check roll/nick stick position
                                        // if nick stick is top or roll stick is left or right --> change parameter setting
                                        // according to roll/nick stick position
                                        if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70)
                                        {
                                                 uint8_t setting = 1; // default
                                                 // nick/roll joystick
                                                 //  _________
                                                 // |2   3   4|
                                                 // |         |
                                                 // |1       5|
                                                 // |         |
                                                 // |         |
                                                 //  ¯¯¯¯¯¯¯¯¯
                                                 // roll stick leftmost and nick stick centered --> setting 1
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 1;
                                                 // roll stick leftmost and nick stick topmost --> setting 2
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 2;
                                                 // roll stick centered an nick stick topmost --> setting 3
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 3;
                                                 // roll stick rightmost and nick stick topmost --> setting 4
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 4;
                                                 // roll stick rightmost and nick stick centered --> setting 5
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 5;
                                                 // update active parameter set in eeprom
                                                 SetActiveParamSet(setting);
                                                 ParamSet_ReadFromEEProm(GetActiveParamSet());
                                                 Servo_Off(); // disable servo output
                                                 SetNeutral(NO_ACC_CALIB);
                                                 Servo_On(); // enable servo output
                                                 Beep(GetActiveParamSet(), 120);
                                        }
                                        else
                                        {
                                                if(ParamSet.Config0 & (CFG0_COMPASS_ACTIVE|CFG0_GPS_ACTIVE))
                                                {
                                                        // if roll stick is centered and nick stick is down
                                                        if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70)
                                                        {
                                                                // nick/roll joystick
                                                                //  _________
                                                                // |         |
                                                                // |         |
                                                                // |         |
                                                                // |         |
                                                                // |    x    |
                                                                //  ¯¯¯¯¯¯¯¯¯
                                                                // enable calibration state of compass
                                                                CompassCalState = 1;
                                                                BeepTime = 1000;
                                                        }
                                                        else // nick and roll are centered
                                                        {
                                                                ParamSet_ReadFromEEProm(GetActiveParamSet());
                                                                Servo_Off(); // disable servo output
                                                                SetNeutral(NO_ACC_CALIB);
                                                                Servo_On(); // enable servo output
                                                                Beep(GetActiveParamSet(), 120);
                                                        }
                                                }
                                                else // nick and roll are centered
                                                {
                                                        ParamSet_ReadFromEEProm(GetActiveParamSet());
                                                        Servo_Off(); // disable servo output
                                                        SetNeutral(NO_ACC_CALIB);
                                                        Servo_On(); // enable servo output
                                                        Beep(GetActiveParamSet(), 120);
                                                }
                                        }
                                }
                        }
                        // and if the yaw stick is in the rightmost position
                        // save the ACC neutral setting to eeprom
                        else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
                        {
                                // gas/yaw joystick is top right
                                //  _________
                                // |        x|
                                // |         |
                                // |         |
                                // |         |
                                // |         |
                                //  ¯¯¯¯¯¯¯¯¯
                                if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
                                {
                                        delay_neutral = 0;
                                        GRN_OFF;
                                        ModelIsFlying = 0;
                                        Servo_Off(); // disable servo output
                                        SetNeutral(ACC_CALIB);
                                        Servo_On(); // enable servo output
                                        Beep(GetActiveParamSet(), 120);
                                }
                        }
                        else delay_neutral = 0;
                }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// gas stick is down
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85)
                {
                        if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
                        {
                                // gas/yaw joystick is bottom right
                                //  _________
                                // |         |
                                // |         |
                                // |         |
                                // |         |
                                // |        x|
                                //  ¯¯¯¯¯¯¯¯¯
                                // Start Motors
                                if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
                                {
                                        delay_startmotors = 200; // do not repeat if once executed
                                        ModelIsFlying = 1;
                                        MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START
                                        SetPointYaw = 0;
                                        ReadingIntegralGyroYaw = 0;
                                        ReadingIntegralGyroNick = ParamSet.GyroAccFactor * (int32_t)AccNick;
                                        ReadingIntegralGyroRoll = ParamSet.GyroAccFactor * (int32_t)AccRoll;
                                        ReadingIntegralGyroNick2 = IntegralGyroNick;
                                        ReadingIntegralGyroRoll2 = IntegralGyroRoll;
                                        IPartNick = 0;
                                        IPartRoll = 0;
                                }
                        }
                        else delay_startmotors = 0; // reset delay timer if sticks are not in this position

                        if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
                        {
                                // gas/yaw joystick is bottom left
                                //  _________
                                // |         |
                                // |         |
                                // |         |
                                // |         |
                                // |x        |
                                //  ¯¯¯¯¯¯¯¯¯
                                // Stop Motors
                                if(++delay_stopmotors > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
                                {
                                        delay_stopmotors = 200; // do not repeat if once executed
                                        ModelIsFlying = 0;
                                        MKFlags &= ~(MKFLAG_MOTOR_RUN);
                                }
                        }
                        else delay_stopmotors = 0; // reset delay timer if sticks are not in this position
                }
                        // remapping of paameters only if the signal rc-sigbnal conditions are good
        } // eof RC_Quality > 150

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// new values from RC
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(!NewPpmData-- || (MKFlags & MKFLAG_EMERGENCY_LANDING) ) // NewData = 0 means new data from RC
        {
                static int16_t stick_nick = 0, stick_roll = 0;

                ParameterMapping(); // remapping params (online poti replacement)

                // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
                stick_nick = (stick_nick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickP) / 4;
                stick_nick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickD;
                StickNick = stick_nick - GPSStickNick;

                stick_roll = (stick_roll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickP) / 4;
                stick_roll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickD;
                StickRoll = stick_roll - GPSStickRoll;

                // mapping of yaw
                StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
                #define YAW_DEAD_RANGE 2
                // (range of -YAW_DEAD_RANGE .. YAW_DEAD_RANGE is set to zero, to avoid unwanted yaw trimming on compass correction)
                if(ParamSet.Config0 & (CFG0_COMPASS_ACTIVE|CFG0_GPS_ACTIVE))
                {
                        if (StickYaw > YAW_DEAD_RANGE) StickYaw-= YAW_DEAD_RANGE;
                        else if (StickYaw< -YAW_DEAD_RANGE) StickYaw += YAW_DEAD_RANGE;
                        else StickYaw = 0;
                }

                // mapping of gas
                StickGas  = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + RC_GAS_OFFSET;// shift to positive numbers

                // update gyro control loop factors
                GyroPFactor = FCParam.GyroP + 10;
                GyroIFactor = FCParam.GyroI;
                GyroYawPFactor = FCParam.GyroYawP + 10;
                GyroYawIFactor = FCParam.GyroYawI;


//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analog control via serial communication
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                #define EXTERNAL_CONTROL_THRESHOLD 128
                #define EXTERNAL_CONTROL_MAXSTICK_LIMIT 100
                if(ExternControl.Config & 0x01 && FCParam.ExternalControl > EXTERNAL_CONTROL_THRESHOLD)
                {
                         StickNick += (int16_t) ExternControl.Nick * (int16_t) ParamSet.StickP;
                         StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.StickP;
                         StickYaw += ExternControl.Yaw;
                         ExternHeightValue =  (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain;
                         if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
                }
                // avoid negative gas value
                if(StickGas < 0) StickGas = 0;

                // disable I part of gyro control feedback
                if(ParamSet.Config0 & CFG0_HEADING_HOLD) GyroIFactor =  0;

                // update max stick positions for nick and roll
                if(abs(StickNick / STICK_GAIN) > MaxStickNick)
                {
                        MaxStickNick = abs(StickNick)/STICK_GAIN;
                        LIMIT_MAX(MaxStickNick, EXTERNAL_CONTROL_MAXSTICK_LIMIT);
                }
                else MaxStickNick--;
                if(abs(StickRoll / STICK_GAIN) > MaxStickRoll)
                {
                        MaxStickRoll = abs(StickRoll)/STICK_GAIN;
                        LIMIT_MAX(MaxStickRoll, EXTERNAL_CONTROL_MAXSTICK_LIMIT);
                }
                else MaxStickRoll--;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.Config1 & CFG1_LOOP_LEFT)  LoopingLeft = 1;
                else
                {
                        if(LoopingLeft) // Hysteresis
                        {
                                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) LoopingLeft = 0;
                        }
                }
                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.Config1 & CFG1_LOOP_RIGHT) LoopingRight = 1;
                else
                {
                        if(LoopingRight) // Hysteresis
                        {
                                if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) LoopingRight = 0;
                        }
                }

                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.Config1 & CFG1_LOOP_UP) LoopingTop = 1;
                else
                {
                        if(LoopingTop)  // Hysteresis
                        {
                                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) LoopingTop = 0;
                        }
                }
                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.Config1 & CFG1_LOOP_DOWN) LoopingDown = 1;
                else
                {
                        if(LoopingDown) // Hysteresis
                        {
                                if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) LoopingDown = 0;
                        }
                }

                if(LoopingLeft || LoopingRight)  LoopingRoll = 1; else LoopingRoll = 0;
                if(LoopingTop  || LoopingDown) { LoopingNick = 1; LoopingRoll = 0; LoopingLeft = 0; LoopingRight = 0;} else  LoopingNick = 0;
        } // End of new RC-Values or Emergency Landing


        if(LoopingRoll ||  LoopingNick)
        {
                LIMIT_MAX(StickGas, ParamSet.LoopGasLimit);
                FunnelCourse = 1;
        }

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// in case of emergency landing
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // set all inputs to save values
        if(MKFlags & MKFLAG_EMERGENCY_LANDING)
        {
                StickYaw = 0;
                StickNick = 0;
                StickRoll = 0;
                StickGas = ParamSet.EmergencyGas;
                GyroPFactor  = 90;
                GyroIFactor  = 120;
                GyroYawPFactor = 90;
                GyroYawIFactor = 120;
                LoopingRoll = 0;
                LoopingNick = 0;
                MaxStickNick = 0;
                MaxStickRoll = 0;
        }

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Trim Gyro-Integrals to ACC-Signals
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        #define BALANCE_NUMBER 256L
        // sum for averaging
        MeanIntegralGyroNick  += IntegralGyroNick;
        MeanIntegralGyroRoll  += IntegralGyroRoll;

        if( LoopingNick || LoopingRoll) // if looping in any direction
        {
                // reset averaging for acc and gyro integral as well as gyro integral acc correction
                MeasurementCounter = 0;

                MeanAccNick = 0;
                MeanAccRoll = 0;

                MeanIntegralGyroNick = 0;
                MeanIntegralGyroRoll = 0;

                ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
                ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;

                AttitudeCorrectionNick = 0;
                AttitudeCorrectionRoll = 0;
        }

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(! LoopingNick && !LoopingRoll && ( (AdValueAccZ > 512) || (MKFlags & MKFLAG_MOTOR_RUN)  ) ) // if not lopping in any direction
        {
                if( FCParam.KalmanK != -1)
                {
                        // determine the deviation of gyro integral from averaged acceleration sensor
                        tmp_long   = (int32_t)(IntegralGyroNick / ParamSet.GyroAccFactor - (int32_t)AccNick);
                        tmp_long   = (tmp_long  * FCParam.KalmanK) / (32 * 16);
                        tmp_long2  = (int32_t)(IntegralGyroRoll / ParamSet.GyroAccFactor - (int32_t)AccRoll);
                        tmp_long2  = (tmp_long2 * FCParam.KalmanK) / (32 * 16);

                        if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
                        {
                                tmp_long  /= 2;
                                tmp_long2 /= 2;
                        }
                        if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active
                        {
                                tmp_long  /= 3;
                                tmp_long2 /= 3;
                        }
                        // limit correction effect
                        LIMIT_MIN_MAX(tmp_long,  -(int32_t)FCParam.KalmanMaxFusion, (int32_t)FCParam.KalmanMaxFusion);
                        LIMIT_MIN_MAX(tmp_long2, -(int32_t)FCParam.KalmanMaxFusion, (int32_t)FCParam.KalmanMaxFusion);
                }
                else
                {
                        // determine the deviation of gyro integral from acceleration sensor
                        tmp_long   = (int32_t)(IntegralGyroNick / ParamSet.GyroAccFactor - (int32_t)AccNick);
                        tmp_long  /= 16;
                        tmp_long2  = (int32_t)(IntegralGyroRoll / ParamSet.GyroAccFactor - (int32_t)AccRoll);
                        tmp_long2 /= 16;

                        if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
                        {
                                tmp_long  /= 3;
                                tmp_long2 /= 3;
                        }
                        if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active
                        {
                                tmp_long  /= 3;
                                tmp_long2 /= 3;
                        }

                        #define BALANCE 32
                        // limit correction effect
                        LIMIT_MIN_MAX(tmp_long,  -BALANCE, BALANCE);
                        LIMIT_MIN_MAX(tmp_long2, -BALANCE, BALANCE);
                }
                // correct current readings
                ReadingIntegralGyroNick -= tmp_long;
                ReadingIntegralGyroRoll -= tmp_long2;
        }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // MeasurementCounter is incremented in the isr of analog.c
        if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
        {
                static int16_t cnt = 0;
                static int8_t last_n_p, last_n_n, last_r_p, last_r_n;
                static int32_t MeanIntegralGyroNick_old, MeanIntegralGyroRoll_old;

                // if not lopping in any direction (this should be always the case,
                // because the Measurement counter is reset to 0 if looping in any direction is active.)
                if(! LoopingNick && !LoopingRoll && !FunnelCourse && ParamSet.DriftComp)
                {
                        // Calculate mean value of the gyro integrals
                        MeanIntegralGyroNick /= BALANCE_NUMBER;
                        MeanIntegralGyroRoll /= BALANCE_NUMBER;

                        // Calculate mean of the acceleration values scaled to the gyro integrals
                        MeanAccNick = (ParamSet.GyroAccFactor * MeanAccNick) / BALANCE_NUMBER;
                        MeanAccRoll = (ParamSet.GyroAccFactor * MeanAccRoll) / BALANCE_NUMBER;

                        // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
                        // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
                        IntegralGyroNickError = (int32_t)(MeanIntegralGyroNick - (int32_t)MeanAccNick);
                        CorrectionNick = IntegralGyroNickError / ParamSet.GyroAccTrim;
                        AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER;
                        // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
                        // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
                        IntegralGyroRollError = (int32_t)(MeanIntegralGyroRoll - (int32_t)MeanAccRoll);
                        CorrectionRoll  = IntegralGyroRollError / ParamSet.GyroAccTrim;
                        AttitudeCorrectionRoll  = CorrectionRoll  / BALANCE_NUMBER;

                        if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.KalmanK == -1) )
                        {
                                AttitudeCorrectionNick /= 2;
                                AttitudeCorrectionRoll /= 2;
                        }

        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // Gyro-Drift ermitteln
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                        // deviation of gyro nick integral (IntegralGyroNick is corrected by averaged acc sensor)
                        IntegralGyroNickError  = IntegralGyroNick2 - IntegralGyroNick;
                        ReadingIntegralGyroNick2 -= IntegralGyroNickError;
                        // deviation of gyro nick integral (IntegralGyroNick is corrected by averaged acc sensor)
                        IntegralGyroRollError = IntegralGyroRoll2 - IntegralGyroRoll;
                        ReadingIntegralGyroRoll2 -= IntegralGyroRollError;

                        if(ParamSet.DriftComp)
                        {
                                if(YawGyroDrift >  BALANCE_NUMBER/2) AdBiasGyroYaw++;
                                if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--;
                        }
                        YawGyroDrift = 0;

                        #define ERROR_LIMIT0 (BALANCE_NUMBER / 2)
                        #define ERROR_LIMIT1 (BALANCE_NUMBER * 2)
                        #define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
                        #define MOVEMENT_LIMIT 20000
        // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
                        cnt = 1;
                        if(IntegralGyroNickError > ERROR_LIMIT1) cnt = 4;
                        CorrectionNick = 0;
                        if((labs(MeanIntegralGyroNick_old - MeanIntegralGyroNick) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8))
                        {
                                if(IntegralGyroNickError >  ERROR_LIMIT2)
                                {
                                        if(last_n_p)
                                        {
                                                cnt += labs(IntegralGyroNickError) / (ERROR_LIMIT2 / 8);
                                                CorrectionNick = IntegralGyroNickError / 8;
                                                if(CorrectionNick > 5000) CorrectionNick = 5000;
                                                AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
                                        }
                                        else last_n_p = 1;
                                }
                                else  last_n_p = 0;
                                if(IntegralGyroNickError < -ERROR_LIMIT2)
                                {
                                        if(last_n_n)
                                        {
                                                cnt += labs(IntegralGyroNickError) / (ERROR_LIMIT2 / 8);
                                                CorrectionNick = IntegralGyroNickError / 8;
                                                if(CorrectionNick < -5000) CorrectionNick = -5000;
                                                AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
                                        }
                                        else last_n_n = 1;
                                }
                                else  last_n_n = 0;
                        }
                        else
                        {
                                cnt = 0;
                                BadCompassHeading = 1000;
                        }
                        if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
                        if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift;
                        // correct Gyro Offsets
                        if(IntegralGyroNickError >  ERROR_LIMIT0)   BiasHiResGyroNick += cnt;
                        if(IntegralGyroNickError < -ERROR_LIMIT0)   BiasHiResGyroNick -= cnt;

        // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
                        cnt = 1;
                        if(IntegralGyroRollError > ERROR_LIMIT1) cnt = 4;
                        CorrectionRoll = 0;
                        if((labs(MeanIntegralGyroRoll_old - MeanIntegralGyroRoll) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8))
                        {
                                if(IntegralGyroRollError >  ERROR_LIMIT2)
                                {
                                        if(last_r_p)
                                        {
                                                cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8);
                                                CorrectionRoll = IntegralGyroRollError / 8;
                                                if(CorrectionRoll > 5000) CorrectionRoll = 5000;
                                                AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
                                        }
                                        else last_r_p = 1;
                                }
                                else  last_r_p = 0;
                                if(IntegralGyroRollError < -ERROR_LIMIT2)
                                {
                                        if(last_r_n)
                                        {
                                                cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8);
                                                CorrectionRoll = IntegralGyroRollError / 8;
                                                if(CorrectionRoll < -5000) CorrectionRoll = -5000;
                                                AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
                                        }
                                        else last_r_n = 1;
                                }
                                else  last_r_n = 0;
                        }
                        else
                        {
                                cnt = 0;
                                BadCompassHeading = 1000;
                        }
                        // correct Gyro Offsets
                        if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
                        if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift;
                        if(IntegralGyroRollError >  ERROR_LIMIT0)   BiasHiResGyroRoll += cnt;
                        if(IntegralGyroRollError < -ERROR_LIMIT0)   BiasHiResGyroRoll -= cnt;

                }
                else // looping is active
                {
                        AttitudeCorrectionRoll  = 0;
                        AttitudeCorrectionNick = 0;
                        FunnelCourse = 0;
                }

                // if GyroIFactor == 0 , for example at Heading Hold, ignore attitude correction
                if(!GyroIFactor)
                {
                        AttitudeCorrectionRoll  = 0;
                        AttitudeCorrectionNick = 0;
                }
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++
                MeanIntegralGyroNick_old = MeanIntegralGyroNick;
                MeanIntegralGyroRoll_old = MeanIntegralGyroRoll;
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++
                // reset variables used for next averaging
                MeanAccNick = 0;
                MeanAccRoll = 0;
                MeanIntegralGyroNick = 0;
                MeanIntegralGyroRoll = 0;
                MeasurementCounter = 0;
        } // end of averaging


// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  Yawing
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(abs(StickYaw) > 15 ) // yaw stick is activated
        {
                BadCompassHeading = 1000;
                if(!(ParamSet.Config0 & CFG0_COMPASS_FIX))
                {
                        UpdateCompassCourse = 1;
                }
        }
        // exponential stick sensitivity in yawring rate
        tmp_int1  = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo  y = ax + bx²
        tmp_int1 += (ParamSet.StickYawP * StickYaw) / 4;
        SetPointYaw = tmp_int1;
        // trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)
        ReadingIntegralGyroYaw -= tmp_int1;
        // limit the effect
        LIMIT_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000)

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  Compass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // compass code is used if Compass option is selected
        if(ParamSet.Config0 & (CFG0_COMPASS_ACTIVE|CFG0_GPS_ACTIVE))
        {
                int16_t w, v, r,correction, error;

                if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) )
                {
                        SetCompassCalState();
                        #ifdef USE_KILLAGREG
                        MM3_Calibrate();
                        #endif
                }
                else
                {
                        #ifdef USE_KILLAGREG
                        static uint8_t updCompass = 0;
                        if (!updCompass--)
                        {
                                updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
                                MM3_Heading();
                        }
                        #endif

                        // get maximum attitude angle
                        w = abs(IntegralGyroNick / 512);
                        v = abs(IntegralGyroRoll / 512);
                        if(v > w) w = v;
                        correction = w / 8 + 1;
                        // calculate the deviation of the yaw gyro heading and the compass heading
                        if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
                        else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;
                        if(abs(GyroYaw) > 128) // spinning fast
                        {
                                error = 0;
                        }
                        if(!BadCompassHeading && w < 25)
                        {
                                YawGyroDrift += error;
                                if(UpdateCompassCourse)
                                {
                                        //BeepTime = 200;
                                        YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
                                        CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR);
                                        UpdateCompassCourse = 0;
                                }
                        }
                        YawGyroHeading += (error * 8) / correction;
                        w = (w * FCParam.CompassYawEffect) / 32;
                        w = FCParam.CompassYawEffect - w;
                        if(w >= 0)
                        {
                                if(!BadCompassHeading)
                                {
                                        v = 64 + (MaxStickNick + MaxStickRoll) / 8;
                                        // calc course deviation
                                        r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
                                        v = (r * w) / v; // align to compass course
                                        // limit yaw rate
                                        w = 3 * FCParam.CompassYawEffect;
                                        if (v > w) v = w;
                                        else if (v < -w) v = -w;
                                        ReadingIntegralGyroYaw += v;
                                }
                                else
                                { // wait a while
                                        BadCompassHeading--;
                                }
                        }
                        else
                        {  // ignore compass at extreme attitudes for a while
                                BadCompassHeading = 500;
                        }
                }
        }

        #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  GPS
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(ParamSet.Config0 & CFG0_GPS_ACTIVE)
        {
                GPS_Main();
                MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
        }
        else
        {
                GPSStickNick = 0;
                GPSStickRoll = 0;
        }
        #endif

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  DebugOutputs
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(!TimerDebugOut--)
        {
                TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz)
                DebugOut.Analog[0]  = (10 * IntegralGyroNick) / GYRO_DEG_FACTOR; // in 0.1 deg
                DebugOut.Analog[1]  = (10 * IntegralGyroRoll) / GYRO_DEG_FACTOR; // in 0.1 deg
                DebugOut.Analog[2]  = (10 * AccNick) / ACC_DEG_FACTOR; // in 0.1 deg
                DebugOut.Analog[3]  = (10 * AccRoll) / ACC_DEG_FACTOR; // in 0.1 deg
                DebugOut.Analog[4]  = GyroYaw;
                DebugOut.Analog[5]  = ReadingHeight/5;
                DebugOut.Analog[6]  = (ReadingIntegralTop / 512);
                DebugOut.Analog[8]  = CompassHeading;
                DebugOut.Analog[9]  = UBat;
                DebugOut.Analog[10] = RC_Quality;
                DebugOut.Analog[11] = YawGyroHeading / GYRO_DEG_FACTOR;
                DebugOut.Analog[19] = CompassCalState;
                DebugOut.Analog[20] = ServoNickValue;
                //DebugOut.Analog[29] = NCSerialDataOkay;
                DebugOut.Analog[30] = GPSStickNick;
                DebugOut.Analog[31] = GPSStickRoll;
        }

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  calculate control feedback from angle (gyro integral) and agular velocity (gyro signal)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        #define TRIM_LIMIT 200
        LIMIT_MIN_MAX(TrimNick, -TRIM_LIMIT, TRIM_LIMIT);
        LIMIT_MIN_MAX(TrimRoll, -TRIM_LIMIT, TRIM_LIMIT);

        if(FunnelCourse)
        {
                IPartNick = 0;
                IPartRoll = 0;
        }

        if(! LoopingNick)
        {
                PPartNick = (IntegralGyroNick * GyroIFactor) / (44000 / STICK_GAIN); // P-Part
        }
        else
        {
                PPartNick = 0;
        }
        PDPartNick = PPartNick + (int32_t)((int32_t)GyroNick * GyroPFactor + (int32_t)TrimNick * 128L) / (256L / STICK_GAIN); //  +D-Part

        if(!LoopingRoll)
        {
                PPartRoll = (IntegralGyroRoll * GyroIFactor) / (44000 / STICK_GAIN); // P-Part
        }
        else
        {
                PPartRoll = 0;
        }
        PDPartRoll = PPartRoll + (int32_t)((int32_t)GyroRoll * GyroPFactor +  (int32_t)TrimRoll * 128L) / (256L / STICK_GAIN); // +D-Part

        PDPartYaw =  (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / STICK_GAIN));

        // limit control feedback
        #define SENSOR_LIMIT  (4096 * 4)
        LIMIT_MIN_MAX(PDPartNick, -SENSOR_LIMIT, SENSOR_LIMIT);
        LIMIT_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
        LIMIT_MIN_MAX(PDPartYaw,  -SENSOR_LIMIT, SENSOR_LIMIT);

        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // Height Control
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        GasMixFraction = StickGas; // take the direct stick command
        // at full LiPo the voltage is higher that gives more trust at the same BL-Control settpoint
        // therefore attenuate the gas proportional to the lipo voltage reserve over the low bat warning level
        // this yields  to a nearly constant effective thrust over lipo discharging at the same stick position
        if(UBat > LowVoltageWarning)
        {
                GasMixFraction = ((uint16_t)GasMixFraction * LowVoltageWarning) / UBat;
        }
        GasMixFraction *= STICK_GAIN; // scale GasMixFraction to enlarge resolution in the motor mixer

        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // Airpressure sensor is enabled
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) && !(LoopingRoll || LoopingNick) )
        {
                #define HOVER_GAS_AVERAGE 4096L         // 4096 * 2ms = 8.1s averaging
                #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8 ms averaging

                int16_t CosAttitude;                                    // for projection of hoover gas
                int16_t HCGas, HeightDeviation;
                static int16_t FilterHCGas = 0;
                static int16_t HeightTrimming = 0;      // rate for change of height setpoint
                static uint8_t HCActive = 0;
                static int16_t StickGasHover = RC_GAS_OFFSET, HoverGas = 0, HoverGasMin = 0, HoverGasMax = 1023;
                static uint32_t HoverGasFilter = 0;
                static uint8_t delay = 100;

                #define BARO_LIMIT_MAX          0x01
                #define BARO_LIMIT_MIN          0x02
                #define BARO_EXPAND_TIME        350             // 350 * 2ms = 0.7s
                static uint8_t BaroFlags = 0;
                static uint16_t BaroExpandActive = 0;

                // get the current hoverpoint
                DebugOut.Analog[21] = HoverGas;
                DebugOut.Analog[18] = ReadingVario;

                // --------- barometer range expansion ------------------
                if(BaroExpandActive) // delay, because of expanding the Baro-Range
                {
                        SumHeight = ReadingHeight * SM_FILTER; // reinit filter for vario
                        ReadingVario = 0;
                        // count down
                        BaroExpandActive--;
                }
                else // expansion not active
                {
                        // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
                        if(AdAirPressure > 923)
                        {       // increase offset
                                if(OCR0A < (255 - EXPANDBARO_OPA_OFFSET_STEP))
                                {
                                        ExpandBaro -= 1;
                                        OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
                                        BeepTime = 300;
                                        BaroExpandActive = BARO_EXPAND_TIME;
                                }
                                else
                                {
                                        BaroFlags |= BARO_LIMIT_MIN;
                                }
                        }
                        // measurement of air pressure close to lower limit and
                        else if(AdAirPressure < 100 )
                        {   // decrease offset
                                if(OCR0A > EXPANDBARO_OPA_OFFSET_STEP)
                                {
                                        ExpandBaro += 1;
                                        OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
                                        BeepTime = 300;
                                        BaroExpandActive = BARO_EXPAND_TIME;
                                }
                                else
                                {
                                        BaroFlags |= BARO_LIMIT_MAX;
                                }
                        }
                        else
                        {       // still ok
                                BaroFlags &= ~(BARO_LIMIT_MIN | BARO_LIMIT_MAX);
                        }
                }// EOF --------- barometer range expansion ------------------


                // if height control is activated by an rc channel
                if(ParamSet.Config0 & CFG0_HEIGHT_SWITCH)
                {       // check if parameter is less than activation threshold
                        if( FCParam.MaxHeight < 50 ) // for 3 or 2-state switch height control is disabled in lowest position
                        {       //height control not active
                                if(!delay--)
                                {
                                        SetPointHeight = ReadingHeight;  // update SetPoint with current reading
                                        HCActive = 0; // disable height control
                                        delay = 1;
                                }
                        }
                        else
                        {       //height control is activated
                                HCActive = 1; // enable height control
                                delay = 200;
                        }
                }
                else // no switchable height control
                {   // the height control is always active and the set point is defined by the parameter
                        if( !(BaroFlags & (BARO_LIMIT_MIN|BARO_LIMIT_MAX)) )
                        {
                                SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain;
                        }
                        HCActive = 1;
                }


                // calculate cos of nick and roll angle used for projection of the vertical hoover gas
                tmp_int1 = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR);  // nick angle in deg
                tmp_int2 = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR);  // roll angle in deg
                CosAttitude = (int16_t)ihypot(tmp_int1, tmp_int2);
                LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
                CosAttitude = c_cos_8192(CosAttitude);  // cos of actual attitude

                if(HCActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING))
                {
                        if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH))
                        {
                                // Holgers 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.

                                HCGas = GasMixFraction; // take current stick gas as neutral point for the height control
                                HeightTrimming = 0;
                        }
                        else // alternative height control
                        {
                                // PD-Control with respect to hover point
                                // the setpoint will be fine adjusted with the gas stick position
                                #define HC_TRIM_UP              0x01
                                #define HC_TRIM_DOWN    0x02
                                static uint8_t HeightTrimmingFlag = 0x00;

                                #define HC_STICKTHRESHOLD 15

                                if(MKFlags & MKFLAG_FLY) // trim setpoint only when flying
                                {   // gas stick is above hover point
                                        if(StickGas > (StickGasHover + HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MAX))
                                        {
                                                if(HeightTrimmingFlag & HC_TRIM_DOWN)
                                                {
                                                        HeightTrimmingFlag &= ~HC_TRIM_DOWN;
                                                        SetPointHeight = ReadingHeight; // update setpoint to current height
                                                }
                                                HeightTrimmingFlag |= HC_TRIM_UP;
                                                HeightTrimming += abs(StickGas - (StickGasHover + HC_STICKTHRESHOLD));
                                        } // gas stick is below hover point
                                        else if(StickGas < (StickGasHover - HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MIN))
                                        {
                                                if(HeightTrimmingFlag & HC_TRIM_UP)
                                                {
                                                        HeightTrimmingFlag &= ~HC_TRIM_UP;
                                                        SetPointHeight = ReadingHeight; // update setpoint to current heigth
                                                }
                                                HeightTrimmingFlag |= HC_TRIM_DOWN;
                                                HeightTrimming -= abs(StickGas - (StickGasHover - HC_STICKTHRESHOLD));
                                        }
                                        else // gas stick in hover range
                                        {
                                                if(HeightTrimmingFlag & (HC_TRIM_UP | HC_TRIM_DOWN))
                                                {
                                                        HeightTrimmingFlag &= ~(HC_TRIM_UP | HC_TRIM_DOWN);
                                                        HeightTrimming = 0;
                                                        SetPointHeight = ReadingHeight; // update setpoint to current height
                                                        if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 500;
                                                }
                                        }
                                        // trim height set point if needed
                                        if(abs(HeightTrimming) > 512)
                                        {
                                                SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/((5 * 512) / 2); // move setpoint
                                                HeightTrimming = 0;
                                                if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 75;
                                                //update hover gas stick value when setpoint is shifted
                                                if(!ParamSet.Height_StickNeutralPoint)
                                                {
                                                        StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value
                                                        StickGasHover = (StickGasHover * UBat) / LowVoltageWarning;
                                                        LIMIT_MIN_MAX(StickGasHover, 70, 150); // reserve some range for trim up and down
                                                }
                                        } // EOF trimming height set point
                                        if(BaroExpandActive) SetPointHeight = ReadingHeight; // update setpoint to current altitude if expanding is active
                                } //if MKFlags & MKFLAG_FLY
                                else // not flying but height control is already active
                                {
                                        SetPointHeight = ReadingHeight - 400; // setpoint should be 4 meters below actual height to avoid a take off
                                        if(ParamSet.Height_StickNeutralPoint) StickGasHover = ParamSet.Height_StickNeutralPoint;
                                        else StickGasHover = RC_GAS_OFFSET;
                                }

                                HCGas = HoverGas;      // take hover gas (neutral point for PD controller)

                        } //EOF alternative height control

                        if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT) )
                        {
                                // from this point the Heigth Control Algorithm is identical for both versions
                                if(BaroExpandActive) // baro range expanding active
                                {
                                        HCGas = HoverGas; // hooer while expanding baro adc range
                                } // EOF // baro range expanding active
                                else // no baro range expanding
                                {
                                        // ------------------------- P-Part ----------------------------
                                        HeightDeviation = (int16_t)(ReadingHeight - SetPointHeight); // positive when too high
                                        tmp_int1 = (HeightDeviation * (int16_t)FCParam.HeightP) / 16; // p-part
                                        HCGas -= tmp_int1;
                                        // ------------------------- D-Part 1: Vario Meter ----------------------------
                                        tmp_int1 = ReadingVario / 8;
                                        if(tmp_int1 > 8) tmp_int1 = 8; // limit quadratic part on upward movement to avoid to much gas reduction
                                        if(tmp_int1 > 0) tmp_int1 = ReadingVario + (tmp_int1 * tmp_int1) / 4;
                                        else            tmp_int1 = ReadingVario - (tmp_int1 * tmp_int1) / 4;
                                        tmp_int1 = (FCParam.HeightD * (int32_t)(tmp_int1)) / 128L; // scale to d-gain parameter
                                        LIMIT_MIN_MAX(tmp_int1, -127, 255);
                                        HCGas -= tmp_int1;
                                        // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
                                        tmp_int1 = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN);
                                        LIMIT_MIN_MAX(tmp_int1, -127, 255);
                                        HCGas -= tmp_int1;

                                        // limit deviation from hover point within the target region
                                        if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HoverGas > 0)) // height setpoint is not changed and hover gas not zero
                                        {
                                                LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hover point
                                        }
                                } // EOF no baro range expanding

                                // ------------------------ D-Part 3: GpsZ  ----------------------------------
                                tmp_int1 = (ParamSet.Height_GPS_Z * (int32_t)NCGpsZ)/128L;
                                LIMIT_MIN_MAX(tmp_int1, -127, 255);
                                HCGas -= tmp_int1;

                                // strech control output by inverse attitude projection 1/cos
                                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;
                                // limit height control gas pd-control output
                                LIMIT_MIN_MAX(FilterHCGas, ParamSet.HeightMinGas * STICK_GAIN, (ParamSet.GasMax - 20) * STICK_GAIN);
                                // limit gas to stick position for limiting height version
                                if(ParamSet.Config2 & CFG2_HEIGHT_LIMIT)
                                {
                                        LIMIT_MAX(FilterHCGas, GasMixFraction);
                                }
                                // set GasMixFraction to HeightControlGasFilter
                                GasMixFraction = FilterHCGas;
                        } // EOF if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT))
                }// EOF height control active
                else // HC not active
                {
                        // update hover gas stick value when HC is not active
                        if(ParamSet.Height_StickNeutralPoint)
                        {
                                StickGasHover = ParamSet.Height_StickNeutralPoint;
                        }
                        else // take real hover stick position
                        {
                                StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value
                                StickGasHover = (StickGasHover * UBat) / LowVoltageWarning;
                        }
                        LIMIT_MIN_MAX(StickGasHover, 70, 150); // reserve some range for trim up and down
                        FilterHCGas = GasMixFraction;           // init filter for HCGas witch current gas mix fraction
                } // EOF HC not active

                // ----------------- Hover Gas Estimation --------------------------------
                // Hover 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((MKFlags & MKFLAG_FLY) && !(MKFlags & MKFLAG_EMERGENCY_LANDING))
                {
                        if(HoverGasFilter == 0)  HoverGasFilter = HOVER_GAS_AVERAGE * (uint32_t)(GasMixFraction); // init estimation
                        if(abs(ReadingVario) < 100) // only on small vertical speed
                        {
                                tmp_long2 = (int32_t)GasMixFraction; // take current thrust
                                tmp_long2 *= CosAttitude;            // apply attitude projection
                                tmp_long2 /= 8192;
                                // average vertical projected thrust
                                if(ModelIsFlying < 2000) // the first 4 seconds
                                {   // reduce the time constant of averaging by factor of 8 to get much faster a stable value
                                        HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/8L);
                                        HoverGasFilter += 8L * tmp_long2;
                                }
                                else if(ModelIsFlying < 4000) // the first 8 seconds
                                {   // reduce the time constant of averaging by factor of 4 to get much faster a stable value
                                        HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L);
                                        HoverGasFilter += 4L * tmp_long2;
                                }
                                else if(ModelIsFlying < 8000) // the first 16 seconds
                                {   // reduce the time constant of averaging by factor of 2 to get much faster a stable value
                                        HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/2L);
                                        HoverGasFilter += 2L * tmp_long2;
                                }
                                else //later
                                {
                                        HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE;
                                        HoverGasFilter += tmp_long2;
                                }
                                HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE);
                                if(ParamSet.Height_HoverBand)
                                {
                                        int16_t band;
                                        band = HoverGas / ParamSet.Height_HoverBand; // the higher the parameter the smaller the range
                                        HoverGasMin = HoverGas - band;
                                        HoverGasMax = HoverGas + band;
                                }
                                else
                                {       // no limit
                                        HoverGasMin = 0;
                                        HoverGasMax = 1023;
                                }
                        } //EOF only on small vertical speed
                }// EOF ----------------- Hover Gas Estimation --------------------------------

        }// EOF ParamSet.Config0 & CFG0_AIRPRESS_SENSOR

        // limit gas to parameter setting
        LIMIT_MIN_MAX(GasMixFraction, (ParamSet.GasMin + 10) * STICK_GAIN, (ParamSet.GasMax - 20) * STICK_GAIN);

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// all BL-Ctrl connected?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(MissingMotor)
        {
                // if we are in the lift off condition
                if( (ModelIsFlying > 1) && (ModelIsFlying < 50) && (GasMixFraction > 0) )
                ModelIsFlying = 1; // keep within lift off condition
                GasMixFraction = ParamSet.GasMin * STICK_GAIN; // reduce gas to min to avoid lift of
        }

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mixer and PI-Controller
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        DebugOut.Analog[7] = GasMixFraction;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Yaw-Fraction
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    YawMixFraction = PDPartYaw - SetPointYaw * STICK_GAIN;     // yaw controller
        #define MIN_YAWGAS (40 * STICK_GAIN)  // yaw also below this gas value
        // limit YawMixFraction
        if(GasMixFraction > MIN_YAWGAS)
        {
                LIMIT_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2));
        }
        else
        {
                LIMIT_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
        }
        tmp_int1 = ParamSet.GasMax * STICK_GAIN;
        LIMIT_MIN_MAX(YawMixFraction, -(tmp_int1 - GasMixFraction), (tmp_int1 - GasMixFraction));

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Axis
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        DiffNick = PDPartNick - StickNick;      // get difference
        if(GyroIFactor) IPartNick += PPartNick - StickNick; // I-part for attitude control
        else IPartNick += DiffNick; // I-part for head holding
        LIMIT_MIN_MAX(IPartNick, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
        NickMixFraction = DiffNick + (IPartNick / Ki); // PID-controller for nick

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Axis
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        DiffRoll = PDPartRoll - StickRoll;      // get difference
        if(GyroIFactor) IPartRoll += PPartRoll - StickRoll; // I-part for attitude control
        else IPartRoll += DiffRoll;  // I-part for head holding
        LIMIT_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
        RollMixFraction = DiffRoll + (IPartRoll / Ki);   // PID-controller for roll

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Limiter
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        tmp_int1 = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64;
        LIMIT_MIN_MAX(NickMixFraction, -tmp_int1, tmp_int1);
        LIMIT_MIN_MAX(RollMixFraction, -tmp_int1, tmp_int1);

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        for(i = 0; i < MAX_MOTORS; i++)
        {
                int16_t tmp;
                if(Mixer.Motor[i][MIX_GAS] > 0) // if gas then mixer
                {
                        tmp =  ((int32_t)GasMixFraction  * Mixer.Motor[i][MIX_GAS] ) / 64L;
                        tmp += ((int32_t)NickMixFraction * Mixer.Motor[i][MIX_NICK]) / 64L;
                        tmp += ((int32_t)RollMixFraction * Mixer.Motor[i][MIX_ROLL]) / 64L;
                        tmp += ((int32_t)YawMixFraction  * Mixer.Motor[i][MIX_YAW] ) / 64L;
                        MotorValue[i] = MotorSmoothing(tmp, MotorValue[i]);  // Spike Filter
                        tmp = MotorValue[i] / STICK_GAIN;
                        LIMIT_MIN_MAX(tmp, ParamSet.GasMin, ParamSet.GasMax);
                        Motor[i].SetPoint = tmp;
                }
                else Motor[i].SetPoint = 0;
        }
}