WebSVN - FlightCtrl - Diff - Rev 1227 and 1240 - /branches/V0.74d_CRD_killagreg_Batteriekappazitaet


/*#######################################################################################
Flight Control
#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <stdlib.h>
#include <avr/io.h>
 
#include "main.h"
#include "eeprom.h"
#include "timer0.h"
#include "analog.h"
#include "fc.h"
#include "uart0.h"
#include "rc.h"
#include "twimaster.h"
#include "timer2.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 CHECK_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;
 
int16_t ReadingHeight = 0;
int16_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,8,150,150,2,10,0,0,0,0,0,0,0,0,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)
{
        while(numbeeps--)
        {
                if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren!
                BeepTime = 100; // 0.1 second
                Delay_ms(250); // blocks 250 ms as pause to next beep,
                // 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.GlobalConfig & CFG_HEIGHT_CONTROL))  // Height Control activated?
    {
                if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) 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)
    {
                // 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);
    }
    // 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);
 
    // reset acc averaging and integrals
    AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick;
    AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
    AccTop  = AdValueAccTop;
    ReadingIntegralTop = AdValueAccTop;
 
        // 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;
 
 
    StartAirPressure = AirPressure;
    HeightD = 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]] + 110;
        Poti2 = PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110;
        Poti3 = PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110;
        Poti4 = PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110;
 
        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 * 3 + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 4L;
        AccRoll  = ((int32_t)AccRoll * 3 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 4L;
        AccTop   = ((int32_t)AccTop  * 3 + ((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;
        CHECK_MIN_MAX(AngleNick, -ANGLE_LIMIT, ANGLE_LIMIT);
 
        AngleRoll = ReadingIntegralGyroRoll;
        CHECK_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.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
        {
                tmp13 = (FilterGyroRoll * AngleNick) / 2048L;
                tmp13 *= FCParam.AxisCoupling2; // 65
                tmp13 /= 4096L;
                CouplingNickRoll = tmp13;
 
                tmp14 = (FilterGyroNick * AngleRoll) / 2048L;
                tmp14 *= FCParam.AxisCoupling2; // 65
                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;
                CHECK_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;
                CHECK_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.GlobalConfig & CFG_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.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(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.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);
                else BeepTime = 1000;
        }
}
 
 
 
/************************************************************************/
/*  MotorControl                                                        */
/************************************************************************/
void MotorControl(void)
{
        int16_t h, tmp_int;
 
        // 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 uint8_t HeightControlActive = 0;
        static int16_t HeightControlGas = 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;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// determine gas value
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        GasMixFraction = StickGas;
    if(GasMixFraction < ParamSet.GasMin + 10) GasMixFraction = ParamSet.GasMin + 10;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// RC-signal is bad
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(RC_Quality < 120)  // the rc-frame signal is not reveived or noisy
        {
                if(!PcAccess) // if also no PC-Access via UART
                {
                        if(BeepModulation == 0xFFFF)
                        {
                                BeepTime = 15000; // 1.5 seconds
                                BeepModulation = 0x0C00;
                        }
                }
                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
                {
                        GasMixFraction = ParamSet.EmergencyGas; // set emergency gas
                        MKFlags |= (MKFLAG_EMERGENCY_LANDING); // ser flag fpr 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 < 120
        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;
                if(GasMixFraction > 40 && (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]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
                if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
                if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
                if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
                //PPM24-Extension
                if(Poti5 < PPM_in[9] + 110)  Poti5++; else if(Poti5 >  PPM_in[9] + 110 && Poti5) Poti5--;
                if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--;
                if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--;
                if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--;
                //limit poti values
                if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
                if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
                if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
                if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
                //PPM24-Extension
                if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255;
                if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
                if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
                if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
 
                // 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());
                                                 SetNeutral(NO_ACC_CALIB);
                                                 Beep(GetActiveParamSet());
                                        }
                                        else
                                        {
                                                if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_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());
                                                                SetNeutral(NO_ACC_CALIB);
                                                                Beep(GetActiveParamSet());
                                                        }
                                                }
                                                else // nick and roll are centered
                                                {
                                                        ParamSet_ReadFromEEProm(GetActiveParamSet());
                                                        SetNeutral(NO_ACC_CALIB);
                                                        Beep(GetActiveParamSet());
                                                }
                                        }
                                }
                        }
                        // 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;
                                        SetNeutral(ACC_CALIB);
                                        Beep(GetActiveParamSet());
                                }
                        }
                        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]];
                // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
                if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
                {
                        if (StickYaw > 2) StickYaw-= 2;
                        else if (StickYaw< -2) StickYaw += 2;
                        else StickYaw = 0;
                }
 
                // mapping of gas
                StickGas  = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers
 
                // update gyro control loop factors
                GyroPFactor = FCParam.GyroP + 10;
                GyroIFactor = FCParam.GyroI;
                GyroYawPFactor = FCParam.GyroP + 10;
                GyroYawIFactor = FCParam.GyroI;
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analog control via serial communication
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
                if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)
                {
                         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;
                }
                if(StickGas < 0) StickGas = 0;
 
                // disable I part of gyro control feedback
                if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor =  0;
 
                // update max stick positions for nick and roll
                if(abs(StickNick / STICK_GAIN) > MaxStickNick)
                {
                        MaxStickNick = abs(StickNick)/STICK_GAIN;
                        if(MaxStickNick > 100) MaxStickNick = 100;
                }
                else MaxStickNick--;
                if(abs(StickRoll / STICK_GAIN) > MaxStickRoll)
                {
                        MaxStickRoll = abs(StickRoll)/STICK_GAIN;
                        if(MaxStickRoll > 100) MaxStickRoll = 100;
                }
                else MaxStickRoll--;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_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.BitConfig & CFG_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.BitConfig & CFG_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.BitConfig & CFG_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)
        {
                if(GasMixFraction > ParamSet.LoopGasLimit) GasMixFraction = 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;
                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
        {
                int32_t tmp_long, tmp_long2;
                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
                        if(tmp_long >  (int32_t)FCParam.KalmanMaxFusion)  tmp_long  = (int32_t)FCParam.KalmanMaxFusion;
                        if(tmp_long < -(int32_t)FCParam.KalmanMaxFusion)  tmp_long  =-(int32_t)FCParam.KalmanMaxFusion;
                        if(tmp_long2 > (int32_t)FCParam.KalmanMaxFusion)  tmp_long2 = (int32_t)FCParam.KalmanMaxFusion;
                        if(tmp_long2 <-(int32_t)FCParam.KalmanMaxFusion)  tmp_long2 =-(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
                        CHECK_MIN_MAX(tmp_long,  -BALANCE, BALANCE);
                        CHECK_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.GlobalConfig & CFG_COMPASS_FIX))
                {
                        UpdateCompassCourse = 1;
                }
        }
        // exponential stick sensitivity in yawring rate
        tmp_int  = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo  y = ax + bx²
        tmp_int += (ParamSet.StickYawP * StickYaw) / 4;
        SetPointYaw = tmp_int;
        // trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)
        ReadingIntegralGyroYaw -= tmp_int;
        // limit the effect
        CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000)
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  Compass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // compass code is used if Compass option is selected
        if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_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.GlobalConfig & CFG_GPS_ACTIVE)
        {
                GPS_Main();
                MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
        }
        else
        {
                GPSStickNick = 0;
                GPSStickRoll = 0;
        }
        #endif
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        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;
                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[24] = GyroNick/2;
        //      DebugOut.Analog[25] = GyroRoll/2;
                DebugOut.Analog[27] = (int16_t)FCParam.KalmanMaxDrift;
        //      DebugOut.Analog[28] = (int16_t)FCParam.KalmanMaxFusion;
                DebugOut.Analog[30] = GPSStickNick;
                DebugOut.Analog[31] = GPSStickRoll;
        }
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  calculate control feedback from angle (gyro integral) and agular velocity (gyro signal)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
        #define TRIM_LIMIT 200
        CHECK_MIN_MAX(TrimNick, -TRIM_LIMIT, TRIM_LIMIT);
        CHECK_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));
 
        //DebugOut.Analog[21] = PDPartNick;
        //DebugOut.Analog[22] = PDPartRoll;
 
        // limit control feedback
        #define SENSOR_LIMIT  (4096 * 4)
        CHECK_MIN_MAX(PDPartNick, -SENSOR_LIMIT, SENSOR_LIMIT);
        CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
        CHECK_MIN_MAX(PDPartYaw,  -SENSOR_LIMIT, SENSOR_LIMIT);
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 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; // reduce gas to min to avoid lift of
        }
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Height Control
// The height control algorithm reduces the gas but does not increase the gas.
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
        GasMixFraction *= STICK_GAIN;
 
        // if height control is activated and no emergency landing is active
        if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && !(MKFlags & MKFLAG_EMERGENCY_LANDING) )
        {
                int tmp_int;
                static uint8_t delay = 100;
                // if height control is activated by an rc channel
                if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
                {       // check if parameter is less than activation threshold
                        if(
                                ( (ParamSet.BitConfig & CFG_HEIGHT_3SWITCH) && ( (FCParam.MaxHeight > 80) && (FCParam.MaxHeight < 140) ) )|| // for 3-state switch height control is only disabled in center position
                                (!(ParamSet.BitConfig & CFG_HEIGHT_3SWITCH) && (FCParam.MaxHeight < 50) ) // for 2-State switch height control is disabled in lower position
                        )
                        {   //hight control not active
                                if(!delay--)
                                {
                                        // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
                                        if( (ReadingAirPressure > 1000) && (OCR0A < 255) )
                                        {   // increase offset
                                                if(OCR0A < 244)
                                                {
                                                        ExpandBaro -= 10;
                                                        OCR0A = PressureSensorOffset - ExpandBaro;
                                                }
                                                else
                                                {
                                                        OCR0A = 254;
                                                }
                                                BeepTime = 300;
                                        delay = 250;
                                        }
                                        // measurement of air pressure close to lower limit and
                                        else if( (ReadingAirPressure < 100) && (OCR0A > 1) )
                                        {   // decrease offset
                                                if(OCR0A > 10)
                                                {
                                                        ExpandBaro += 10;
                                                        OCR0A = PressureSensorOffset - ExpandBaro;
                                                }
                                                else
                                                {
                                                        OCR0A = 1;
                                                }
                                                BeepTime = 300;
                                        delay = 250;
                                        }
                                        else
                                        {
                                                SetPointHeight = ReadingHeight - 20;  // update SetPoint with current reading
                                                HeightControlActive = 0; // disable height control
                                                delay = 1;
                                        }
                                }
                        }
                        else
                        {       //hight control not active
                                HeightControlActive = 1; // enable height control
                                delay = 200;
                        }
                }
                else // no switchable height control
                {
                        SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
                        HeightControlActive = 1;
                }
                // get current height
                h = ReadingHeight;
                // if current height is above the setpoint reduce gas
                if((h > SetPointHeight) && HeightControlActive)
                {
                        // height difference -> P control part
                        h = ((h - SetPointHeight) * (int16_t) FCParam.HeightP) / (16 / STICK_GAIN);
                        h = GasMixFraction - h; // reduce gas
                        // height gradient --> D control part
                        //h -= (HeightD * FCParam.HeightD) / (8 / STICK_GAIN);  // D control part
                        h -= (HeightD) / (8 / STICK_GAIN);  // D control part
                        // acceleration sensor effect
                        tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN);
                        if(tmp_int > 70 * STICK_GAIN)        tmp_int =   70 * STICK_GAIN;
                        else if(tmp_int < -(70 * STICK_GAIN)) tmp_int = -(70 * STICK_GAIN);
                        h -= tmp_int;
                        // update height control gas
                        HeightControlGas = (HeightControlGas*15 + h) / 16;
                        // limit gas reduction
                        if(HeightControlGas < ParamSet.HeightMinGas * STICK_GAIN)
                        {
                                if(GasMixFraction >= ParamSet.HeightMinGas * STICK_GAIN) HeightControlGas = ParamSet.HeightMinGas * STICK_GAIN;
                                // allows landing also if gas stick is reduced below min gas on height control
                                if(GasMixFraction < ParamSet.HeightMinGas * STICK_GAIN) HeightControlGas = GasMixFraction;
                        }
                        // limit gas to stick setting
                        if(HeightControlGas > GasMixFraction) HeightControlGas = GasMixFraction;
                        GasMixFraction = HeightControlGas;
                }
        }
        // limit gas to parameter setting
        if(GasMixFraction > (ParamSet.GasMax - 20) * STICK_GAIN) GasMixFraction = (ParamSet.GasMax - 20) * STICK_GAIN;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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)
        {
                CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2));
        }
        else
        {
                CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
        }
        tmp_int = ParamSet.GasMax * STICK_GAIN;
        CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - 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
        CHECK_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
        CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
        RollMixFraction = DiffRoll + (IPartRoll / Ki);   // PID-controller for roll
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Limiter
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64;
        CHECK_MIN_MAX(NickMixFraction, -tmp_int, tmp_int);
        CHECK_MIN_MAX(RollMixFraction, -tmp_int, tmp_int);
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 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;
                        CHECK_MIN_MAX(tmp, ParamSet.GasMin, ParamSet.GasMax);
                        Motor[i].SetPoint = tmp;
                }
                else Motor[i].SetPoint = 0;
        }
}
 
 
Subversion Repositories FlightCtrl

(root)/branches/V0.74d_CRD_killagreg_Batteriekappazitaet_Lion/fc.c @ 2152 - Rev 1227 → 1240
