Subversion Repositories FlightCtrl

/*#######################################################################################

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);