WebSVN - FlightCtrl - Diff - Rev 1179 and 1180 - /branches/V0.72p Code Redesign killagreg/fc.c

 #include <avr/io.h>
 #include "main.h"
 #include "eeprom.h"
-#include "timer0.h"
-#include "_Settings.h"
+#include "timer0.h"
 #include "analog.h"
 #include "fc.h"
-#include "uart.h"
+#include "uart0.h"
 #include "rc.h"
 #include "twimaster.h"
 #include "timer2.h"
 #ifdef USE_KILLAGREG
 #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 Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
+int16_t GyroNick, GyroRoll, GyroYaw;
-// gyro neutral readings
+// gyro bias
-int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
+int16_t BiasHiResGyroNick = 0, BiasHiResGyroRoll = 0, AdBiasGyroYaw = 0;
-int16_t StartNeutralRoll = 0, StartNeutralNick = 0;
-// mean accelerations
+// accelerations
-int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
+int16_t AccNick, AccRoll, AccTop;
 // neutral acceleration readings
-volatile int16_t NeutralAccX=0, NeutralAccY=0;
+int16_t AdBiasAccNick = 0, AdBiasAccRoll = 0;
+volatile float AdBiasAccTop = 0;
+// the additive gyro rate corrections according to the axis coupling
+int16_t TrimNick, TrimRoll;
 volatile float NeutralAccZ = 0;
 // attitude gyro integrals
-int32_t IntegralNick = 0,IntegralNick2 = 0;
+int32_t IntegralGyroNick = 0,IntegralGyroNick2 = 0;
-int32_t IntegralRoll = 0,IntegralRoll2 = 0;
+int32_t IntegralGyroRoll = 0,IntegralGyroRoll2 = 0;
-int32_t IntegralYaw = 0;
+int32_t IntegralGyroYaw = 0;
-int32_t Reading_IntegralGyroNick = 0, Reading_IntegralGyroNick2 = 0;
+int32_t ReadingIntegralGyroNick = 0, ReadingIntegralGyroNick2 = 0;
-int32_t Reading_IntegralGyroRoll = 0, Reading_IntegralGyroRoll2 = 0;
+int32_t ReadingIntegralGyroRoll = 0, ReadingIntegralGyroRoll2 = 0;
-int32_t Reading_IntegralGyroYaw = 0;
+int32_t ReadingIntegralGyroYaw = 0;
-int32_t MeanIntegralNick;
+int32_t MeanIntegralGyroNick;
-int32_t MeanIntegralRoll;
+int32_t MeanIntegralGyroRoll;
 // attitude acceleration integrals
-int32_t IntegralAccNick = 0, IntegralAccRoll = 0;
+int32_t MeanAccNick = 0, MeanAccRoll = 0;
-volatile int32_t Reading_Integral_Top = 0;
+volatile int32_t ReadingIntegralTop = 0;
 // compass course
-volatile int16_t CompassHeading = -1; // negative angle indicates invalid data.
+int16_t CompassHeading = -1; // negative angle indicates invalid data.
-volatile int16_t CompassCourse = -1;
+int16_t CompassCourse = -1;
-volatile int16_t CompassOffCourse = 0;
+int16_t CompassOffCourse = 0;
-volatile uint8_t CompassCalState = 0;
+uint8_t CompassCalState = 0;
 uint8_t FunnelCourse = 0;
 uint16_t BadCompassHeading = 500;
-int32_t YawGyroHeading;
+int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass
 int16_t YawGyroDrift;
-int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;
+int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;
 // MK flags
 uint16_t Model_Is_Flying = 0;
+// MK flags
-volatile uint8_t MKFlags = 0;
+uint16_t ModelIsFlying = 0;
+uint8_t  MKFlags = 0;
 int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
-float Gyro_P_Factor;
+uint8_t GyroPFactor, GyroIFactor; // the PD factors for the attitude control
+int16_t Ki = 10300 / 33;
 float Gyro_I_Factor;
 int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
-int16_t  DiffNick, DiffRoll;
 int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
 // setpoints for motors
 // setpoints for motors
-volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left;
+volatile uint8_t Motor1, Motor2, Motor3, Motor4, Motor5, Motor6, Motor7, Motor8;
 // stick values derived by rc channels readings
-int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0;
+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 GPS_Nick = 0, GPS_Roll = 0;
 int16_t AttitudeCorrectionRoll = 0, AttitudeCorrectionNick = 0;
 int16_t MaxStickNick = 0, MaxStickRoll = 0;
-// stick values derived by uart inputs
+uint8_t LoopingNick = 0, LoopingRoll = 0;
+}
 /************************************************************************/
 /*  Neutral Readings                                                    */
 /************************************************************************/
-void SetNeutral(void)
+void SetNeutral(uint8_t AccAdjustment)
+{
+        uint8_t i;
+        int32_t Sum_1, Sum_2 = 0, Sum_3;
+        Servo_Off(); // disable servo output
-{
-        NeutralAccX = 0;
+        AdBiasAccNick = 0;
-        NeutralAccY = 0;
+        AdBiasAccRoll = 0;
+        AdBiasAccTop = 0;
-        NeutralAccZ = 0;
-    AdNeutralNick = 0;
+    BiasHiResGyroNick = 0;
-        AdNeutralRoll = 0;
+        BiasHiResGyroRoll = 0;
+        AdBiasGyroYaw = 0;
-        AdNeutralYaw = 0;
-    FCParam.Yaw_PosFeedback = 0;
+    FCParam.AxisCoupling1 = 0;
+    FCParam.AxisCoupling2 = 0;
     FCParam.Yaw_NegFeedback = 0;
+    ExpandBaro = 0;
-    ExpandBaro = 0;
-    CalibMean();
+        // sample values with bias set to zero
+    Delay_ms_Mess(100);
-    Delay_ms_Mess(100);
+    if(BoardRelease == 13) SearchDacGyroOffset();
         CalibMean();
     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;
-        AdNeutralNick = AdValueGyrNick;
+        Sum_3 = 0;
+    for(i=0; i < GYRO_BIAS_AVERAGE; i++)
+    {
-        AdNeutralRoll  = AdValueGyrRoll;
+                Delay_ms_Mess(10);
-        AdNeutralYaw   = AdValueGyrYaw;
+                Sum_1 += AdValueGyroNick * HIRES_GYRO_AMPLIFY;
-        StartNeutralRoll  = AdNeutralRoll;
+                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);
-        StartNeutralNick = AdNeutralNick;
-    if(GetParamWord(PID_ACC_NICK) > 1023)
+    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++)
+                {
-                NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY;
+                        Delay_ms_Mess(10);
+                        Sum_1 += AdValueAccNick;
-                NeutralAccX = abs(Mean_AccNick) / ACC_AMPLIFY;
+                        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);
-                NeutralAccZ = Current_AccZ;
+                SetParamWord(PID_ACC_TOP,  (uint16_t)AdBiasAccTop);
     }
-    else
+    else // restore from eeprom
     {
-                NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK);
+                AdBiasAccNick = (int16_t)GetParamWord(PID_ACC_NICK);
-            NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL);
+            AdBiasAccRoll = (int16_t)GetParamWord(PID_ACC_ROLL);
-            NeutralAccZ = (int16_t)GetParamWord(PID_ACC_Z);
+            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);
-        Reading_IntegralGyroNick = 0;
+    // reset acc averaging and integrals
+    AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick;
-    Reading_IntegralGyroNick2 = 0;
+    AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
-    Reading_IntegralGyroRoll = 0;
+    AccTop  = AdValueAccTop;
+    ReadingIntegralTop = AdValueAccTop;
-    Reading_IntegralGyroRoll2 = 0;
-    Reading_IntegralGyroYaw = 0;
+        // and gyro readings
-    Reading_GyroNick = 0;
+        GyroNick = 0;
-    Reading_GyroRoll = 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;
-    Reading_GyroYaw = 0;
+    ReadingIntegralGyroRoll2 = IntegralGyroRoll;
+    ReadingIntegralGyroYaw = 0;
     Delay_ms_Mess(100);
     StartAirPressure = AirPressure;
+    HeightD = 0;
-    HeightD = 0;
-    Reading_Integral_Top = 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;
     CompassCourse = CompassHeading;
+    BeepTime = 50;
     BeepTime = 50;
         TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L;
+        TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L;
         TurnOver180Roll =  ((int32_t) ParamSet.AngleTurnOverRoll *  2500L) +15000L;
+    ExternHeightValue = 0;
-    ExternHeightValue = 0;
-    GPS_Nick = 0;
+    GPSStickNick = 0;
-    GPS_Roll = 0;
-    YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
+    GPSStickRoll = 0;
     YawGyroDrift = 0;
+    MKFlags |= MKFLAG_CALIBRATE;
-    MKFlags |= MKFLAG_CALIBRATE;
-        FCParam.Kalman_K = -1;
+        FCParam.KalmanK = -1;
-        FCParam.Kalman_MaxDrift = ParamSet.DriftComp * 16;
+        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
-        FCParam.Kalman_MaxFusion = 32;
+        RC_Quality = 100;
+}
 /************************************************************************/
 /*  Averaging Measurement Readings                                      */
 /************************************************************************/
+void Mean(void)
-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;
-{
-    static int32_t tmpl,tmpl2;
+                if(AdValueGyroRoll > 2015) GyroRoll = +2000;
- // Get offset corrected gyro readings (~ to angular velocity)
-    Reading_GyroYaw   = AdNeutralYaw   - AdValueGyrYaw;
+        }
-    Reading_GyroRoll  = AdValueGyrRoll - AdNeutralRoll;
-    Reading_GyroNick  = AdValueGyrNick - AdNeutralNick;
+        GyroYaw   = AdBiasGyroYaw - AdValueGyroYaw;
-// Acceleration Sensor
+        // Acceleration Sensor
-        // sliding average sensor readings
+        // lowpass acc measurement and scale AccNick/AccRoll by a factor of ACC_AMPLIFY to have a better resolution
-        Mean_AccNick  = ((int32_t)Mean_AccNick * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 2L;
+        AccNick  = ((int32_t)AccNick * 3 + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 4L;
-        Mean_AccRoll  = ((int32_t)Mean_AccRoll * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 2L;
+        AccRoll  = ((int32_t)AccRoll * 3 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 4L;
-        Mean_AccTop   = ((int32_t)Mean_AccTop * 1 + ((int32_t)AdValueAccTop)) / 2L;
+        AccTop   = ((int32_t)AccTop  * 3 + ((int32_t)AdValueAccTop)) / 4L;
-        // sum sensor readings for later averaging
+        // sum acc sensor readings for later averaging
-    IntegralAccNick += ACC_AMPLIFY * AdValueAccNick;
+    MeanAccNick  += ACC_AMPLIFY * AdValueAccNick;
-    IntegralAccRoll  += ACC_AMPLIFY * AdValueAccRoll;
+    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);
-    NaviAccNick += AdValueAccNick;
+        AngleRoll = ReadingIntegralGyroRoll;
-    NaviAccRoll  += AdValueAccRoll;
-    NaviCntAcc++;
+        CHECK_MIN_MAX(AngleRoll, -ANGLE_LIMIT, ANGLE_LIMIT);
-// Yaw
         // calculate yaw gyro integral (~ to rotation angle)
-        Reading_IntegralGyroYaw  += Reading_GyroYaw;
+        // Yaw
-        YawGyroHeading += Reading_GyroYaw;
+        // 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
-    if(YawGyroHeading >= (360L * YAW_GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * YAW_GYRO_DEG_FACTOR;  // 360° Wrap
+                tmp14 /= 4096L;
-        if(YawGyroHeading < 0)                             YawGyroHeading += 360L * YAW_GYRO_DEG_FACTOR;
+                CouplingRollNick = tmp14;
+                tmp14 -= tmp13;
+                YawGyroHeading += tmp14;
-        // Coupling fraction
+                if(!FCParam.AxisCouplingYawCorrection)  ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw
         if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
                 tmpl = ((GyroYaw + tmp14) * AngleNick) / 2048L;
-                tmpl = (Reading_GyroYaw * Reading_IntegralGyroNick) / 2048L;
-                tmpl *= FCParam.Yaw_PosFeedback;
-                tmpl /= 4096L;
-                tmpl2 = ( Reading_GyroYaw * Reading_IntegralGyroRoll) / 2048L;
+                tmpl *= FCParam.AxisCoupling1;
-                tmpl2 *= FCParam.Yaw_PosFeedback;
-                tmpl2 /= 4096L;
-                if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1;
-        }
-        else  tmpl = tmpl2 = 0;
-// Roll
-        Reading_GyroRoll += tmpl;
-        Reading_GyroRoll += (tmpl2 * FCParam.Yaw_NegFeedback) / 512L;
-        Reading_IntegralGyroRoll2 += Reading_GyroRoll;
-        Reading_IntegralGyroRoll +=  Reading_GyroRoll - AttitudeCorrectionRoll;
-        if(Reading_IntegralGyroRoll > TurnOver180Roll)
-        {
-                Reading_IntegralGyroRoll  = -(TurnOver180Roll - 10000L);
-                Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll;
-        }
-        if(Reading_IntegralGyroRoll < -TurnOver180Roll)
-        {
+                tmpl /= 4096L;
-                Reading_IntegralGyroRoll =  (TurnOver180Roll - 10000L);
                 Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll;
                 tmpl2 = ((GyroYaw + tmp14) * AngleRoll) / 2048L;
-        if(AdValueGyrRoll < 15)   Reading_GyroRoll = -1000;
+                tmpl2 *= FCParam.AxisCoupling1;
-        if(AdValueGyrRoll <  7)   Reading_GyroRoll = -2000;
+                tmpl2 /= 4096L;
-        if(BoardRelease == 10)
+                if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1;
+                TrimNick = -tmpl2 + tmpl / 100L;
                 TrimRoll = tmpl - tmpl2 / 100L;
+        }
+        else
-                if(AdValueGyrRoll > 1010) Reading_GyroRoll = +1000;
+        {
+                CouplingNickRoll = 0;
-                if(AdValueGyrRoll > 1017) Reading_GyroRoll = +2000;
+                CouplingRollNick = 0;
+                TrimNick = 0;
-        }
+                TrimRoll = 0;
+        }
-        else
-        {
+        // Yaw
-                if(AdValueGyrRoll > 2020) Reading_GyroRoll = +1000;
-                if(AdValueGyrRoll > 2034) Reading_GyroRoll = +2000;
+    // limit YawGyroHeading proportional to 0° to 360°
-        }
+    if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR))      YawGyroHeading -= 360L * GYRO_DEG_FACTOR;  // 360° Wrap
-// Nick
+        if(YawGyroHeading < 0)                                          YawGyroHeading += 360L * GYRO_DEG_FACTOR;
         Reading_GyroNick -= tmpl2;
-        Reading_GyroNick -= (tmpl*FCParam.Yaw_NegFeedback) / 512L;
+        // Roll
-        Reading_IntegralGyroNick2 += Reading_GyroNick;
+        ReadingIntegralGyroRoll2 += FilterGyroRoll + TrimRoll;
-        Reading_IntegralGyroNick  += Reading_GyroNick - AttitudeCorrectionNick;
+        ReadingIntegralGyroRoll  += FilterGyroRoll + TrimRoll- AttitudeCorrectionRoll;
-        if(Reading_IntegralGyroNick > TurnOver180Nick)
+        if(ReadingIntegralGyroRoll > TurnOver180Roll)
+        {
+                ReadingIntegralGyroRoll  = -(TurnOver180Roll - 10000L);
+                ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;
-         Reading_IntegralGyroNick = -(TurnOver180Nick - 25000L);
+        }
-         Reading_IntegralGyroNick2 = Reading_IntegralGyroNick;
+        if(ReadingIntegralGyroRoll < -TurnOver180Roll)
-        }
+        {
-        if(Reading_IntegralGyroNick < -TurnOver180Nick)
+                ReadingIntegralGyroRoll =  (TurnOver180Roll - 10000L);
-        {
+                ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;
-         Reading_IntegralGyroNick =  (TurnOver180Nick - 25000L);
+        }
          Reading_IntegralGyroNick2 = Reading_IntegralGyroNick;
-        }
+        // Nick
-        if(AdValueGyrNick < 15)   Reading_GyroNick = -1000;
+        ReadingIntegralGyroNick2 += FilterGyroNick + TrimNick;
-        if(AdValueGyrNick <  7)   Reading_GyroNick = -2000;
+        ReadingIntegralGyroNick  += FilterGyroNick + TrimNick - AttitudeCorrectionNick;
-        if(BoardRelease == 10)
+        if(ReadingIntegralGyroNick > TurnOver180Nick)
+        {
+                ReadingIntegralGyroNick = -(TurnOver180Nick - 25000L);
+                ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
-                if(AdValueGyrNick > 1010) Reading_GyroNick = +1000;
+        }
+        if(ReadingIntegralGyroNick < -TurnOver180Nick)
-                if(AdValueGyrNick > 1017) Reading_GyroNick = +2000;
+        {
-        }
-        else
-        {
-                if(AdValueGyrNick > 2020) Reading_GyroNick = +1000;
-                if(AdValueGyrNick > 2034) Reading_GyroNick = +2000;
-        }
+                ReadingIntegralGyroNick =  (TurnOver180Nick - 25000L);
+                ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
+        }
-// start ADC again to capture measurement values for the next loop
+    IntegralGyroYaw    = ReadingIntegralGyroYaw;
-    ADC_Enable();
+    IntegralGyroNick   = ReadingIntegralGyroNick;
+    IntegralGyroRoll   = ReadingIntegralGyroRoll;
+    IntegralGyroNick2  = ReadingIntegralGyroNick2;
+    IntegralGyroRoll2  = ReadingIntegralGyroRoll2;
-    IntegralYaw    = Reading_IntegralGyroYaw;
+        #define D_LIMIT 128
+        if(FCParam.GyroD)
+        {
+                d2Nick = (HiResGyroNick - Last_GyroNick); // change of gyro rate
-    IntegralNick  = Reading_IntegralGyroNick;
+                Last_GyroNick = (Last_GyroNick + HiResGyroNick) / 2;
-    IntegralRoll   = Reading_IntegralGyroRoll;
-    IntegralNick2 = Reading_IntegralGyroNick2;
+                CHECK_MIN_MAX(d2Nick, -D_LIMIT, D_LIMIT);
-    IntegralRoll2  = Reading_IntegralGyroRoll2;
+                GyroNick += (d2Nick * (int16_t)FCParam.GyroD) / 16;
-        if((ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER) && !Looping_Nick && !Looping_Roll)
-        {
-                if(Reading_GyroNick > 200)       Reading_GyroNick += 4 * (Reading_GyroNick - 200);
+                d2Roll = (HiResGyroRoll - Last_GyroRoll); // change of gyro rate
-                else if(Reading_GyroNick < -200) Reading_GyroNick += 4 * (Reading_GyroNick + 200);
+                Last_GyroRoll = (Last_GyroRoll + HiResGyroRoll) / 2;
-                if(Reading_GyroRoll > 200)        Reading_GyroRoll  += 4 * (Reading_GyroRoll - 200);
-                else if(Reading_GyroRoll < -200)  Reading_GyroRoll  += 4 * (Reading_GyroRoll + 200);
+                CHECK_MIN_MAX(d2Roll, -D_LIMIT, D_LIMIT);
-        }
+                GyroRoll += (d2Roll * (int16_t)FCParam.GyroD) / 16;
-}
-/************************************************************************/
+                HiResGyroNick += (d2Nick * (int16_t)FCParam.GyroD);
-/*  Averaging Measurement Readings  for Calibration                     */
+                HiResGyroRoll += (d2Roll * (int16_t)FCParam.GyroD);
-/************************************************************************/
+        }
-void CalibMean(void)
-{
+        // Increase the roll/nick rate virtually proportional to the coupling to suppress a faster rotation
-        if(BoardRelease == 13) SearchGyroOffset();
-    // stop ADC to avoid changing values during calculation
+        if(FilterGyroNick > 0)  TrimNick += ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L;
-        ADC_Disable();
-        Reading_GyroNick = AdValueGyrNick;
+        else                    TrimNick -= ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L;
+        if(FilterGyroRoll > 0)  TrimRoll += ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L;
-        Reading_GyroRoll  = AdValueGyrRoll;
+        else                    TrimRoll -= ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L;
         Reading_GyroYaw   = AdValueGyrYaw;
         // increase the nick/roll rates virtually from the threshold of 245 to slow down higher rotation rates
-        Mean_AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick;
+        if((ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER) && ! LoopingNick && !LoopingRoll)
         Mean_AccRoll  = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
-        Mean_AccTop   = (int32_t)AdValueAccTop;
+                if(FilterGyroNick > 256)                GyroNick += 1 * (FilterGyroNick - 256);
-    // start ADC (enables internal trigger so that the ISR in analog.c
+                else if(FilterGyroNick < -256)  GyroNick += 1 * (FilterGyroNick + 256);
+                if(FilterGyroRoll > 256)        GyroRoll += 1 * (FilterGyroRoll - 256);
-    // updates the readings once)
+                else if(FilterGyroRoll < -256)  GyroRoll += 1 * (FilterGyroRoll + 256);
-    ADC_Enable();
+        }
-        TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L;
+}
-        TurnOver180Roll =  (int32_t) ParamSet.AngleTurnOverRoll  * 2500L;
-}
+/************************************************************************/
-/************************************************************************/
+/*  Transmit Motor Data via I2C                                         */
-/*  Transmit Motor Data via I2C                                         */
 /************************************************************************/
+void SendMotorData(void)
+{
+    if(!(MKFlags & MKFLAG_MOTOR_RUN))
+    {
+                #ifdef USE_QUADRO
+                Motor1 = 0;
+                Motor2 = 0;
+                Motor3 = 0;
-void SendMotorData(void)
+                Motor4 = 0;
-{
+                if(MotorTest[0]) Motor1 = MotorTest[0];
-    if(!(MKFlags & MKFLAG_MOTOR_RUN))
+                if(MotorTest[1]) Motor2 = MotorTest[1];
-    {
+                if(MotorTest[2]) Motor3 = MotorTest[2];
+                if(MotorTest[3]) Motor4 = MotorTest[3];
+                #else
+                Motor1 = 0;
+                Motor2 = 0;
+                Motor3 = 0;
+                Motor4 = 0;
+                Motor5 = 0;
+                Motor6 = 0;
+                Motor7 = 0;
+                Motor8 = 0;
+                if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];}
+                if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];}
+                if(MotorTest[1]) {Motor5 = MotorTest[1]; Motor6 = MotorTest[1];}
+                if(MotorTest[2]) {Motor7 = MotorTest[2]; Motor8 = MotorTest[2];}
-        Motor_Rear = 0;
+                #endif
-        Motor_Front = 0;
+                MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
         Motor_Right = 0;
-        Motor_Left = 0;
+        #ifdef USE_QUADRO
         if(MotorTest[0]) Motor_Front = MotorTest[0];
-        if(MotorTest[1]) Motor_Rear  = MotorTest[1];
+        DebugOut.Analog[12] = Motor1; // Front
-        if(MotorTest[2]) Motor_Left  = MotorTest[2];
+        DebugOut.Analog[13] = Motor2; // Rear
-        if(MotorTest[3]) Motor_Right = MotorTest[3];
+        DebugOut.Analog[14] = Motor4; // Left
-        MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
+        DebugOut.Analog[15] = Motor3; // Right
         #else // OCTO Motor addresses are counted clockwise starting at the head
-    DebugOut.Analog[12] = Motor_Front;
+        DebugOut.Analog[12] = (Motor1 + Motor2) / 2;
-    DebugOut.Analog[13] = Motor_Rear;
+        DebugOut.Analog[13] = (Motor5 + Motor6) / 2;
-    DebugOut.Analog[14] = Motor_Left;
+        DebugOut.Analog[14] = (Motor7 + Motor8) / 2;
-    DebugOut.Analog[15] = Motor_Right;
+        DebugOut.Analog[15] = (Motor3 + Motor4) / 2;
         #endif
     //Start I2C Interrupt Mode
     twi_state = TWI_STATE_MOTOR_TX;
     I2C_Start();
+}
+/************************************************************************/
-/************************************************************************/
+/*  Map the parameter to poti values                                    */
-/*  Maps the parameter to poti values                                   */
+/************************************************************************/
-/************************************************************************/
+void ParameterMapping(void)
 void ParameterMapping(void)
         if(RC_Quality > 160) // do the mapping of RC-Potis only if the rc-signal is ok
-        if(RC_Quality > 160) // do the mapping of RC-Potis only if the rc-signal is ok
+        // else the last updated values are used
         // else the last updated values are used
                  //update poti values by rc-signals
-                 //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_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;}
-                #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(FCParam.MaxHeight,ParamSet.MaxHeight);
+                CHK_POTI_MM(FCParam.HeightD,ParamSet.HeightD,0,100);
-                CHK_POTI_MM(FCParam.Height_D,ParamSet.Height_D,0,100);
+                CHK_POTI_MM(FCParam.HeightP,ParamSet.HeightP,0,100);
+                CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect);
-                CHK_POTI_MM(FCParam.Height_P,ParamSet.Height_P,0,100);
+                CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect);
-                CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect);
+                CHK_POTI_MM(FCParam.GyroP,ParamSet.GyroP,10,255);
-                CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect);
+                CHK_POTI(FCParam.GyroI,ParamSet.GyroI);
-                CHK_POTI_MM(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
+                CHK_POTI(FCParam.GyroD,ParamSet.GyroD);
-                CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I);
+                CHK_POTI(FCParam.IFactor,ParamSet.IFactor);
                 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 = (float) FCParam.I_Factor * FACTOR_I;
+                Ki = 10300 / ( FCParam.IFactor + 1 );
+        }
+}
 /************************************************************************/
 /*  MotorControl                                                        */
 /************************************************************************/
 void MotorControl(void)
+{
-        int16_t MotorValue, pd_result, h, tmp_int;
+        int16_t MotorValue, h, tmp_int;
+        // Mixer Fractions that are combined for Motor Control
-        int16_t YawMixFraction, GasMixFraction;
+        int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;
+        // PID controller variables
+        int16_t DiffNick, DiffRoll;
+        int16_t PDPartNick, PDPartRoll, PDPartYaw, PPartNick, PPartRoll;
-        static int32_t SumNick = 0, SumRoll = 0;
+        static int32_t IPartNick = 0, IPartRoll = 0;
         static int32_t SetPointYaw = 0;
-        static int32_t IntegralErrorNick = 0;
+        static int32_t IntegralGyroNickError = 0, IntegralGyroRollError = 0;
-        static int32_t IntegralErrorRoll = 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 int8_t  TimerDebugOut = 0;
         static uint16_t UpdateCompassCourse = 0;
+        // high resolution motor values for smoothing of PID motor outputs
+        static int16_t MotorValue1 = 0, MotorValue2 = 0, MotorValue3 = 0, MotorValue4 = 0;
+        #ifndef USE_QUADRO
-        static int32_t CorrectionNick, CorrectionRoll;
+        static int16_t MotorValue5 = 0, MotorValue6 = 0, MotorValue7 = 0, MotorValue8 = 0;
+        #endif
         Mean();
         GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // determine gas value
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMixFraction = StickGas;
-    if(GasMixFraction < ParamSet.Gas_Min + 10) GasMixFraction = ParamSet.Gas_Min + 10;
+    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
+                                BeepTime = 15000; // 1.5 seconds
-                         BeepModulation = 0x0C00;
+                                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()
+                        MKFlags &= ~(MKFLAG_MOTOR_RUN|MKFLAG_EMERGENCY_LANDING); // clear motor run flag that stop the motors in SendMotorData()
                 }
                 RED_ON; // set red led
-                if(Model_Is_Flying > 1000)  // wahrscheinlich in der Luft --> langsam absenken
+                if(ModelIsFlying > 1000)  // wahrscheinlich in der Luft --> langsam absenken
+                {
                         GasMixFraction = ParamSet.EmergencyGas; // set emergency gas
                         MKFlags |= (MKFLAG_EMERGENCY_LANDING); // ser flag fpr emergency landing
                 MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
                 // reset emergency timer
                 RcLostTimer = ParamSet.EmergencyGasDuration * 50;
                 if(GasMixFraction > 40 && (MKFlags & MKFLAG_MOTOR_RUN) )
+                {
-                        if(Model_Is_Flying < 0xFFFF) Model_Is_Flying++;
+                        if(ModelIsFlying < 0xFFFF) ModelIsFlying++;
+                }
-                if(Model_Is_Flying < 256)
+                if(ModelIsFlying < 256)
+                {
-                        SumNick = 0;
+                        IPartNick = 0;
-                        SumRoll = 0;
+                        IPartRoll = 0;
                         StickYaw = 0;
-                        if(Model_Is_Flying == 250)
+                        if(ModelIsFlying == 250)
+                        {
                                 UpdateCompassCourse = 1;
-                                Reading_IntegralGyroYaw = 0;
+                                ReadingIntegralGyroYaw = 0;
                                 SetPointYaw = 0;
+                        }
+                }
                 else MKFlags |= (MKFLAG_FLY); // set fly flag
                                 //  ¯¯¯¯¯¯¯¯¯
                                 if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
+                                {
                                         delay_neutral = 0;
                                         GRN_OFF;
-                                        Model_Is_Flying = 0;
+                                        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)
+                                        {
                                                  // 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();
+                                                 SetNeutral(NO_ACC_CALIB);
                                                  Beep(GetActiveParamSet());
+                                        }
                                         else
+                                        {
                                                 if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
                                                                 BeepTime = 1000;
+                                                        }
                                                         else // nick and roll are centered
+                                                        {
                                                                 ParamSet_ReadFromEEProm(GetActiveParamSet());
-                                                                SetNeutral();
+                                                                SetNeutral(NO_ACC_CALIB);
                                                                 Beep(GetActiveParamSet());
+                                                        }
+                                                }
                                                 else // nick and roll are centered
+                                                {
                                                         ParamSet_ReadFromEEProm(GetActiveParamSet());
-                                                        SetNeutral();
+                                                        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;
-                                        SetParamWord(PID_ACC_NICK, 0xFFFF); // make value invalid
-                                        Model_Is_Flying = 0;
+                                        ModelIsFlying = 0;
-                                        SetNeutral();
+                                        SetNeutral(ACC_CALIB);
-                                        // Save ACC neutral settings to eeprom
-                                        SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX);
-                                        SetParamWord(PID_ACC_ROLL,  (uint16_t)NeutralAccY);
-                                        SetParamWord(PID_ACC_Z,     (uint16_t)NeutralAccZ);
                                         Beep(GetActiveParamSet());
+                                }
+                        }
                         else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // gas stick is down
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85)
+                {
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// and yaw stick is rightmost --> start motors
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         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
-                                        Model_Is_Flying = 1;
+                                        ModelIsFlying = 1;
                                         MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START
                                         SetPointYaw = 0;
-                                        Reading_IntegralGyroYaw = 0;
+                                        ReadingIntegralGyroYaw = 0;
-                                        Reading_IntegralGyroNick = 0;
+                                        ReadingIntegralGyroNick = ParamSet.GyroAccFactor * (int32_t)AccNick;
-                                        Reading_IntegralGyroRoll = 0;
+                                        ReadingIntegralGyroRoll = ParamSet.GyroAccFactor * (int32_t)AccRoll;
-                                        Reading_IntegralGyroNick2 = IntegralNick;
+                                        ReadingIntegralGyroNick2 = IntegralGyroNick;
-                                        Reading_IntegralGyroRoll2 = IntegralRoll;
+                                        ReadingIntegralGyroRoll2 = IntegralGyroRoll;
-                                        SumNick = 0;
+                                        IPartNick = 0;
-                                        SumRoll = 0;
+                                        IPartRoll = 0;
+                                }
+                        }
                         else delay_startmotors = 0; // reset delay timer if sticks are not in this position
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// and yaw stick is leftmost --> stop motors
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         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
-                                        Model_Is_Flying = 0;
+                                        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
+        {
                 ParameterMapping(); // remapping params (online poti replacement)
                 // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
-                StickNick = (StickNick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_P) / 4;
+                StickNick = (StickNick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickP) / 4;
-                StickNick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_D;
+                StickNick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickD;
-                StickNick -= (GPS_Nick);
+                StickNick -= (GPSStickNick);
-                StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
+                StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickP) / 4;
-                StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
+                StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickD;
-                StickRoll -= (GPS_Roll);
+                StickRoll -= (GPSStickRoll);
-                // direct mapping of yaw and gas
+                // 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;
+                }
-                StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
+                // mapping of gas
                 StickGas  = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers
                 // update gyro control loop factors
-                Gyro_P_Factor = ((float) FCParam.Gyro_P + 10.0) / (256.0 / STICK_GAIN);
+                GyroPFactor = FCParam.GyroP + 10;
-                Gyro_I_Factor = ((float) FCParam.Gyro_I) / (44000 / STICK_GAIN);
+                GyroIFactor = FCParam.GyroI;
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Analog control via serial communication
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)
+                {
-                         StickNick += (int16_t) ExternControl.Nick * (int16_t) ParamSet.Stick_P;
+                         StickNick += (int16_t) ExternControl.Nick * (int16_t) ParamSet.StickP;
-                         StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P;
+                         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(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
-                if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor =  0;
                 // avoid negative scaling factors
-                if(Gyro_P_Factor < 0) Gyro_P_Factor = 0;
+                if(StickGas < 0) StickGas = 0;
                 if(Gyro_I_Factor < 0) Gyro_I_Factor = 0;
                 // disable I part of gyro control feedback
                 if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor =  0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_LEFT)  Looping_Left = 1;
+                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_LEFT)  LoopingLeft = 1;
                 else
+                {
-                        if(Looping_Left) // Hysteresis
+                        if(LoopingLeft) // Hysteresis
+                        {
-                                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0;
+                                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) Looping_Right = 1;
+                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_RIGHT) LoopingRight = 1;
                 else
+                {
-                        if(Looping_Right) // Hysteresis
+                        if(LoopingRight) // Hysteresis
+                        {
-                                if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0;
+                                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) Looping_Top = 1;
+                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_UP) LoopingTop = 1;
                 else
                 {
-                        if(Looping_Top)  // Hysteresis
+                        if(LoopingTop)  // Hysteresis
+                        {
-                                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0;
+                                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) Looping_Down = 1;
+                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_DOWN) LoopingDown = 1;
                 else
                 {
-                        if(Looping_Down) // Hysteresis
+                        if(LoopingDown) // Hysteresis
+                        {
-                                if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0;
+                                if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) LoopingDown = 0;
                         }
+                }
-                if(Looping_Left || Looping_Right)   Looping_Roll = 1; else Looping_Roll = 0;
+                if(LoopingLeft || LoopingRight)  LoopingRoll = 1; else LoopingRoll = 0;
-                if(Looping_Top  || Looping_Down) {Looping_Nick = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Nick = 0;
+                if(LoopingTop  || LoopingDown) { LoopingNick = 1; LoopingRoll = 0; LoopingLeft = 0; LoopingRight = 0;} else  LoopingNick = 0;
+        } // End of new RC-Values or Emergency Landing
         } // End of new RC-Values or Emergency Landing
         if(LoopingRoll ||  LoopingNick)
         if(Looping_Roll || Looping_Nick)
         if(MKFlags & MKFLAG_EMERGENCY_LANDING)
+        {
                 StickYaw = 0;
                 StickNick = 0;
                 StickRoll = 0;
-                Gyro_P_Factor  = (float) 100 / (256.0 / STICK_GAIN);
+                GyroPFactor  = 90;
-                Gyro_I_Factor = (float) 120 / (44000 / STICK_GAIN);
+                GyroIFactor = 120;
-                Looping_Roll = 0;
+                LoopingRoll = 0;
-                Looping_Nick = 0;
+                LoopingNick = 0;
                 MaxStickNick = 0;
                 MaxStickRoll = 0;
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Trim Gyro-Integrals to ACC-Signals
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         #define BALANCE_NUMBER 256L
         // sum for averaging
-        MeanIntegralNick  += IntegralNick;
+        MeanIntegralGyroNick  += IntegralGyroNick;
-        MeanIntegralRoll  += IntegralRoll;
+        MeanIntegralGyroRoll  += IntegralGyroRoll;
-        if(Looping_Nick || Looping_Roll) // if looping in any direction
+        if( LoopingNick || LoopingRoll) // if looping in any direction
         {
                 // reset averaging for acc and gyro integral as well as gyro integral acc correction
                 MeasurementCounter = 0;
-                IntegralAccNick = 0;
+                MeanAccNick = 0;
-                IntegralAccRoll = 0;
+                MeanAccRoll = 0;
-                MeanIntegralNick = 0;
+                MeanIntegralGyroNick = 0;
-                MeanIntegralRoll = 0;
+                MeanIntegralGyroRoll = 0;
-                Reading_IntegralGyroNick2 = Reading_IntegralGyroNick;
+                ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
-                Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll;
+                ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;
                 AttitudeCorrectionNick = 0;
                 AttitudeCorrectionRoll = 0;
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        if(!Looping_Nick && !Looping_Roll) // if not lopping in any direction
+        if(! LoopingNick && !LoopingRoll && (AdValueAccZ > 512)) // if not lopping in any direction or rapid falling
         {
                 int32_t tmp_long, tmp_long2;
-                if(FCParam.Kalman_K != -1)
+                if( FCParam.KalmanK != -1)
+                {
                         // determine the deviation of gyro integral from averaged acceleration sensor
+                        {
                                 tmp_long  /= 3;
                                 tmp_long2 /= 3;
+                        }
                         // limit correction effect
-                        if(tmp_long >  (int32_t)FCParam.Kalman_MaxFusion)  tmp_long  = (int32_t)FCParam.Kalman_MaxFusion;
+                        if(tmp_long >  (int32_t)FCParam.KalmanMaxFusion)  tmp_long  = (int32_t)FCParam.KalmanMaxFusion;
-                        if(tmp_long < -(int32_t)FCParam.Kalman_MaxFusion)  tmp_long  =-(int32_t)FCParam.Kalman_MaxFusion;
+                        if(tmp_long < -(int32_t)FCParam.KalmanMaxFusion)  tmp_long  =-(int32_t)FCParam.KalmanMaxFusion;
-                        if(tmp_long2 > (int32_t)FCParam.Kalman_MaxFusion)  tmp_long2 = (int32_t)FCParam.Kalman_MaxFusion;
+                        if(tmp_long2 > (int32_t)FCParam.KalmanMaxFusion)  tmp_long2 = (int32_t)FCParam.KalmanMaxFusion;
-                        if(tmp_long2 <-(int32_t)FCParam.Kalman_MaxFusion)  tmp_long2 =-(int32_t)FCParam.Kalman_MaxFusion;
+                        if(tmp_long2 <-(int32_t)FCParam.KalmanMaxFusion)  tmp_long2 =-(int32_t)FCParam.KalmanMaxFusion;
+                }
                 else
+                {
-                        // determine the deviation of gyro integral from averaged acceleration sensor
+                        // determine the deviation of gyro integral from acceleration sensor
-                        tmp_long   =  (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
+                        tmp_long   = (int32_t)(IntegralGyroNick / ParamSet.GyroAccFactor - (int32_t)AccNick);
                         tmp_long  /= 16;
-                        tmp_long2  = (int32_t)(IntegralRoll   / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
+                        tmp_long2  = (int32_t)(IntegralGyroRoll / ParamSet.GyroAccFactor - (int32_t)AccRoll);
                         tmp_long2 /= 16;
                         if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
+                        {
                                 tmp_long2 /= 3;
+                        }
                         #define BALANCE 32
-                        // limit correction effect
-                        if(tmp_long >  BALANCE)  tmp_long  = BALANCE;
+                        // limit correction effect
-                        if(tmp_long < -BALANCE)  tmp_long  =-BALANCE;
-                        if(tmp_long2 > BALANCE)  tmp_long2 = BALANCE;
+                        CHECK_MIN_MAX(tmp_long,  -BALANCE, BALANCE);
-                        if(tmp_long2 <-BALANCE)  tmp_long2 =-BALANCE;
+                        CHECK_MIN_MAX(tmp_long2, -BALANCE, BALANCE);
+                }
                 // correct current readings
-                Reading_IntegralGyroNick -= tmp_long;
+                ReadingIntegralGyroNick -= tmp_long;
-                Reading_IntegralGyroRoll -= tmp_long2;
+                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 MeanIntegralNick_old, MeanIntegralRoll_old;
+                static int32_t MeanIntegralGyroNick_old, MeanIntegralGyroRoll_old;
-                // if not lopping in any direction (this should be alwais the case,
+                // 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(!Looping_Nick && !Looping_Roll && !FunnelCourse)
+                if(! LoopingNick && !LoopingRoll && !FunnelCourse && ParamSet.DriftComp)
                 {
                         // Calculate mean value of the gyro integrals
-                        MeanIntegralNick /= BALANCE_NUMBER;
+                        MeanIntegralGyroNick /= BALANCE_NUMBER;
-                        MeanIntegralRoll  /= BALANCE_NUMBER;
+                        MeanIntegralGyroRoll /= BALANCE_NUMBER;
-                        // Calculate mean of the acceleration values
+                        // Calculate mean of the acceleration values scaled to the gyro integrals
-                        IntegralAccNick = (ParamSet.GyroAccFactor * IntegralAccNick) / BALANCE_NUMBER;
+                        MeanAccNick = (ParamSet.GyroAccFactor * MeanAccNick) / BALANCE_NUMBER;
-                        IntegralAccRoll  = (ParamSet.GyroAccFactor * IntegralAccRoll ) / BALANCE_NUMBER;
+                        MeanAccRoll = (ParamSet.GyroAccFactor * MeanAccRoll) / BALANCE_NUMBER;
                         // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
                         // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
-                        IntegralErrorNick = (int32_t)(MeanIntegralNick - (int32_t)IntegralAccNick);
+                        IntegralGyroNickError = (int32_t)(MeanIntegralGyroNick - (int32_t)MeanAccNick);
-                        CorrectionNick = IntegralErrorNick / ParamSet.GyroAccTrim;
+                        CorrectionNick = IntegralGyroNickError / ParamSet.GyroAccTrim;
                         AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER;
                         // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
                         // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
-                        IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
+                        IntegralGyroRollError = (int32_t)(MeanIntegralGyroRoll - (int32_t)MeanAccRoll);
-                        CorrectionRoll  = IntegralErrorRoll / ParamSet.GyroAccTrim;
+                        CorrectionRoll  = IntegralGyroRollError / ParamSet.GyroAccTrim;
                         AttitudeCorrectionRoll  = CorrectionRoll  / BALANCE_NUMBER;
-                        if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.Kalman_K == -1) )
+                        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;
-                        // deviation of gyro nick integral (IntegralNick is corrected by averaged acc sensor)
+                        ReadingIntegralGyroNick2 -= IntegralGyroNickError;
-                        IntegralErrorNick  = IntegralNick2 - IntegralNick;
+                        // deviation of gyro nick integral (IntegralGyroNick is corrected by averaged acc sensor)
+                        IntegralGyroRollError = IntegralGyroRoll2 - IntegralGyroRoll;
-                        Reading_IntegralGyroNick2 -= IntegralErrorNick;
+                        ReadingIntegralGyroRoll2 -= IntegralGyroRollError;
                         // deviation of gyro nick integral (IntegralNick is corrected by averaged acc sensor)
-                        IntegralErrorRoll = IntegralRoll2 - IntegralRoll;
+                        if(ParamSet.DriftComp)
                         Reading_IntegralGyroRoll2 -= IntegralErrorRoll;
                                 if(YawGyroDrift >  BALANCE_NUMBER/2) AdBiasGyroYaw++;
-                        if(YawGyroDrift >  BALANCE_NUMBER/2) AdNeutralYaw++;
+                                if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--;
                         if(YawGyroDrift < -BALANCE_NUMBER/2) AdNeutralYaw--;
                         YawGyroDrift = 0;
                         #define ERROR_LIMIT  (BALANCE_NUMBER * 4)
                         #define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
                         #define MOVEMENT_LIMIT 20000
         // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
-                        cnt = 1;// + labs(IntegralErrorNick) / 4096;
+                        cnt = 1;// + labs(IntegralGyroNickError) / 4096;
                         CorrectionNick = 0;
-                        if((labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT) || (FCParam.Kalman_MaxDrift > 3* 16))
+                        if((labs(MeanIntegralGyroNick_old - MeanIntegralGyroNick) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8))
+                        {
-                                if(IntegralErrorNick >  ERROR_LIMIT2)
+                                if(IntegralGyroNickError >  ERROR_LIMIT2)
+                                {
                                         if(last_n_p)
+                                        {
-                                                cnt += labs(IntegralErrorNick) / ERROR_LIMIT2;
+                                                cnt += labs(IntegralGyroNickError) / (ERROR_LIMIT2 / 8);
-                                                CorrectionNick = IntegralErrorNick / 8;
+                                                CorrectionNick = IntegralGyroNickError / 8;
                                                 if(CorrectionNick > 5000) CorrectionNick = 5000;
                                                 AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
                                         }
                                         else last_n_p = 1;
+                                }
                                 else  last_n_p = 0;
-                                if(IntegralErrorNick < -ERROR_LIMIT2)
+                                if(IntegralGyroNickError < -ERROR_LIMIT2)
+                                {
                                         if(last_n_n)
+                        {
                                 cnt = 0;
                                 BadCompassHeading = 1000;
+                        }
                         if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
-                        if(cnt * 16 > FCParam.Kalman_MaxDrift) cnt = FCParam.Kalman_MaxDrift / 16;
+                        if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift;
                         // correct Gyro Offsets
-                        if(IntegralErrorNick >  ERROR_LIMIT)   AdNeutralNick += cnt;
+                        if(IntegralGyroNickError >  ERROR_LIMIT)   BiasHiResGyroNick += cnt;
-                        if(IntegralErrorNick < -ERROR_LIMIT)   AdNeutralNick -= cnt;
+                        if(IntegralGyroNickError < -ERROR_LIMIT)   BiasHiResGyroNick -= cnt;
         // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
-                        cnt = 1;// + labs(IntegralErrorNick) / 4096;
+                        cnt = 1;// + labs(IntegralGyroNickError) / 4096;
                         CorrectionRoll = 0;
-                        if((labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT) || (FCParam.Kalman_MaxDrift > 3 * 16))
+                        if((labs(MeanIntegralGyroRoll_old - MeanIntegralGyroRoll) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8))
                         {
-                                if(IntegralErrorRoll >  ERROR_LIMIT2)
+                                if(IntegralGyroRollError >  ERROR_LIMIT2)
+                                {
                                         if(last_r_p)
                                         {
-                                                cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
+                                                cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8);
-                                                CorrectionRoll = IntegralErrorRoll / 8;
+                                                CorrectionRoll = IntegralGyroRollError / 8;
                                                 if(CorrectionRoll > 5000) CorrectionRoll = 5000;
                                                 AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
+                                        }
                                         else last_r_p = 1;
+                                }
                                 else  last_r_p = 0;
-                                if(IntegralErrorRoll < -ERROR_LIMIT2)
+                                if(IntegralGyroRollError < -ERROR_LIMIT2)
+                                {
                                         if(last_r_n)
                                         {
-                                                cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
+                                                cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8);
-                                                CorrectionRoll = IntegralErrorRoll / 8;
+                                                CorrectionRoll = IntegralGyroRollError / 8;
                                                 if(CorrectionRoll < -5000) CorrectionRoll = -5000;
                                                 AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
+                                        }
                                         else last_r_n = 1;
                                 cnt = 0;
                                 BadCompassHeading = 1000;
+                        }
                         // correct Gyro Offsets
                         if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
-                        if(cnt * 16 > FCParam.Kalman_MaxDrift) cnt = FCParam.Kalman_MaxDrift / 16;
+                        if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift;
-                        if(IntegralErrorRoll >  ERROR_LIMIT)   AdNeutralRoll += cnt;
+                        if(IntegralGyroRollError >  ERROR_LIMIT)   BiasHiResGyroRoll += cnt;
-                        if(IntegralErrorRoll < -ERROR_LIMIT)   AdNeutralRoll -= cnt;
+                        if(IntegralGyroRollError < -ERROR_LIMIT)   BiasHiResGyroRoll -= cnt;
+                }
                 else // looping is active
+                {
                         AttitudeCorrectionRoll  = 0;
                         AttitudeCorrectionNick = 0;
                         FunnelCourse = 0;
                 }
-                // if Gyro_I_Factor == 0 , for example at Heading Hold, ignore attitude correction
+                // if GyroIFactor == 0 , for example at Heading Hold, ignore attitude correction
-                if(!Gyro_I_Factor)
+                if(!GyroIFactor)
+                {
                         AttitudeCorrectionRoll  = 0;
                         AttitudeCorrectionNick = 0;
                 }
         // +++++++++++++++++++++++++++++++++++++++++++++++++++++
-                MeanIntegralNick_old = MeanIntegralNick;
+                MeanIntegralGyroNick_old = MeanIntegralGyroNick;
-                MeanIntegralRoll_old  = MeanIntegralRoll;
+                MeanIntegralGyroRoll_old = MeanIntegralGyroRoll;
         // +++++++++++++++++++++++++++++++++++++++++++++++++++++
-                // reset variables used for averaging
+                // reset variables used for next averaging
-                IntegralAccNick = 0;
+                MeanAccNick = 0;
-                IntegralAccRoll = 0;
+                MeanAccRoll = 0;
-                MeanIntegralNick = 0;
+                MeanIntegralGyroNick = 0;
-                MeanIntegralRoll = 0;
+                MeanIntegralGyroRoll = 0;
                 MeasurementCounter = 0;
+                {
                         UpdateCompassCourse = 1;
+                }
+        }
         // exponential stick sensitivity in yawring rate
-        tmp_int  = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo  y = ax + bx²
+        tmp_int  = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo  y = ax + bx²
-        tmp_int += (ParamSet.Yaw_P * StickYaw) / 4;
+        tmp_int += (ParamSet.StickYawP * StickYaw) / 4;
         SetPointYaw = tmp_int;
-        // trimm drift of Reading_IntegralGyroYaw with SetPointYaw(StickYaw)
+        // trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)
-        Reading_IntegralGyroYaw -= tmp_int;
+        ReadingIntegralGyroYaw -= tmp_int;
         // limit the effect
-        if(Reading_IntegralGyroYaw > 50000) Reading_IntegralGyroYaw = 50000;
-        if(Reading_IntegralGyroYaw <-50000) Reading_IntegralGyroYaw =-50000;
+        CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000)
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Compass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                 MM3_Heading();
+                        }
                         #endif
                         // get maximum attitude angle
-                        w = abs(IntegralNick / 512);
+                        w = abs(IntegralGyroNick / 512);
-                        v = abs(IntegralRoll / 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 / YAW_GYRO_DEG_FACTOR)) % 360) - 180;
+                        else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;
-                        if(UpdateCompassCourse)
+                        if(abs(GyroYaw) > 128) // spinning fast
+                        {
-                                error = 0;
-                                YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
+                                error = 0;
+                        }
                         if(!BadCompassHeading && w < 25)
+                        {
                                 YawGyroDrift += error;
                                 if(UpdateCompassCourse)
+                                {
+                                        BeepTime = 200;
-                                        BeepTime = 200;
+                                        YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
-                                        CompassCourse = (YawGyroHeading / YAW_GYRO_DEG_FACTOR);
+                                        CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR);
                                         UpdateCompassCourse = 0;
+                                }
+                        }
                         YawGyroHeading += (error * 8) / correction;
+                        {
                                 if(!BadCompassHeading)
+                                {
                                         v = 64 + (MaxStickNick + MaxStickRoll) / 8;
                                         // calc course deviation
-                                        r = ((540 + (YawGyroHeading / YAW_GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
+                                        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;
-                                        Reading_IntegralGyroYaw += v;
+                                        ReadingIntegralGyroYaw += v;
+                                }
                                 else
                                 { // wait a while
                                         BadCompassHeading--;
+                                }
                 GPS_Main();
                 MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
+        }
         else
+        {
-                GPS_Nick = 0;
+                GPSStickNick = 0;
-                GPS_Roll = 0;
+                GPSStickRoll = 0;
+        }
         #endif
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(!TimerDebugOut--)
+        {
                 TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz)
-                DebugOut.Analog[0]  = IntegralNick / ParamSet.GyroAccFactor;
+                DebugOut.Analog[0]  = (10 * IntegralGyroNick) / GYRO_DEG_FACTOR; // in 0.1 deg
-                DebugOut.Analog[1]  = IntegralRoll / ParamSet.GyroAccFactor;
+                DebugOut.Analog[1]  = (10 * IntegralGyroRoll) / GYRO_DEG_FACTOR; // in 0.1 deg
-                DebugOut.Analog[2]  = Mean_AccNick;
+                DebugOut.Analog[2]  = (10 * AccNick) / ACC_DEG_FACTOR; // in 0.1 deg
-                DebugOut.Analog[3]  = Mean_AccRoll;
+                DebugOut.Analog[3]  = (10 * AccRoll) / ACC_DEG_FACTOR; // in 0.1 deg
-                DebugOut.Analog[4]  = Reading_GyroYaw;
+                DebugOut.Analog[4]  = GyroYaw;
                 DebugOut.Analog[5]  = ReadingHeight;
-                DebugOut.Analog[6]  = (Reading_Integral_Top / 512);
+                DebugOut.Analog[6]  = (ReadingIntegralTop / 512);
                 DebugOut.Analog[8]  = CompassHeading;
                 DebugOut.Analog[9]  = UBat;
                 DebugOut.Analog[10] = RC_Quality;
-                DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
+                DebugOut.Analog[11] = YawGyroHeading / GYRO_DEG_FACTOR;
-                //DebugOut.Analog[16] = Mean_AccTop;
+                DebugOut.Analog[19] = CompassCalState;
-                //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
+        //      DebugOut.Analog[24] = GyroNick/2;
-                //DebugOut.Analog[18] = FromNaviCtrl_Value.OsdBar;
+        //      DebugOut.Analog[25] = GyroRoll/2;
+                DebugOut.Analog[27] = (int16_t)FCParam.KalmanMaxDrift;
-                DebugOut.Analog[27] = (int16_t)FCParam.Kalman_MaxDrift;
+        //      DebugOut.Analog[28] = (int16_t)FCParam.KalmanMaxFusion;
-                DebugOut.Analog[29] = (int16_t)FCParam.Kalman_K;
+        //      DebugOut.Analog[29] = (int16_t)FCParam.KalmanK;
-                DebugOut.Analog[30] = GPS_Nick;
+                DebugOut.Analog[30] = GPSStickNick;
-                DebugOut.Analog[31] = GPS_Roll;
                 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(Looping_Nick) Reading_GyroNick = Reading_GyroNick * Gyro_P_Factor;
+        if(! LoopingNick)
+        {
+                PPartNick = (IntegralGyroNick * GyroIFactor) / (44000 / STICK_GAIN); // P-Part
+        }
+        else
-        else Reading_GyroNick = IntegralNick * Gyro_I_Factor + Reading_GyroNick * Gyro_P_Factor;
+        {
+                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
+        // octo has a double yaw momentum because of the doubled motor number
+        // therefore double D-Part and halfen P-Part for the same result
+        #ifdef USE_OCTO
+        PDPartYaw =  (int32_t)(GyroYaw * 4 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (4 * (44000 / STICK_GAIN));
-        if(Looping_Roll) Reading_GyroRoll = Reading_GyroRoll * Gyro_P_Factor;
+        #else
-        else Reading_GyroRoll = IntegralRoll * Gyro_I_Factor + Reading_GyroRoll * Gyro_P_Factor;
+        PDPartYaw =  (int32_t)(GyroYaw * 2 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (2 * (44000 / STICK_GAIN));
-        Reading_GyroYaw = Reading_GyroYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2;
+        #endif
-        DebugOut.Analog[21] = Reading_GyroNick;
-        DebugOut.Analog[22] = Reading_GyroRoll;
+        //DebugOut.Analog[21] = PDPartNick;
-        // limit control feedback
+        //DebugOut.Analog[22] = PDPartRoll;
-        #define MAX_SENSOR  (4096 * STICK_GAIN)
-        if(Reading_GyroNick >  MAX_SENSOR) Reading_GyroNick =  MAX_SENSOR;
-        if(Reading_GyroNick < -MAX_SENSOR) Reading_GyroNick = -MAX_SENSOR;
+        // limit control feedback
-        if(Reading_GyroRoll >  MAX_SENSOR) Reading_GyroRoll =  MAX_SENSOR;
+        #define SENSOR_LIMIT  (4096 * 4)
-        if(Reading_GyroRoll < -MAX_SENSOR) Reading_GyroRoll = -MAX_SENSOR;
+        CHECK_MIN_MAX(PDPartNick, -SENSOR_LIMIT, SENSOR_LIMIT);
-        if(Reading_GyroYaw  >  MAX_SENSOR) Reading_GyroYaw  =  MAX_SENSOR;
+        CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
                 // get current height
                 h = ReadingHeight;
                 // if current height is above the setpoint reduce gas
                 if((h > SetPointHeight) && HeightControlActive)
+                {
-                        // GasMixFraction - HightDeviation * P  - HeightChange * D - ACCTop * DACC
                         // height difference -> P control part
-                        h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / (16 / STICK_GAIN);
+                        h = ((h - SetPointHeight) * (int16_t) FCParam.HeightP) / (16 / STICK_GAIN);
                         h = GasMixFraction - h; // reduce gas
                         // height gradient --> D control part
-                        //h -= (HeightD * FCParam.Height_D) / (8 / STICK_GAIN);  // 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 = ((Reading_Integral_Top / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN);
+                        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.Height_MinGas * STICK_GAIN)
+                        if(HeightControlGas < ParamSet.HeightMinGas * STICK_GAIN)
+                        {
-                                if(GasMixFraction >= ParamSet.Height_MinGas * STICK_GAIN) HeightControlGas = ParamSet.Height_MinGas * 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.Height_MinGas * STICK_GAIN) HeightControlGas = GasMixFraction;
+                                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.Gas_Max - 20) * STICK_GAIN) GasMixFraction = (ParamSet.Gas_Max - 20) * STICK_GAIN;
+        if(GasMixFraction > (ParamSet.GasMax - 20) * STICK_GAIN) GasMixFraction = (ParamSet.GasMax - 20) * STICK_GAIN;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mixer and PI-Controller
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DebugOut.Analog[7] = GasMixFraction;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Yaw-Fraction
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    YawMixFraction = Reading_GyroYaw - SetPointYaw * STICK_GAIN;     // yaw controller
+    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)
+        {
-                if(YawMixFraction >  (GasMixFraction / 2)) YawMixFraction = GasMixFraction / 2;
-                if(YawMixFraction < -(GasMixFraction / 2)) YawMixFraction = -(GasMixFraction / 2);
+                CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2));
+        }
         else
+        {
-                if(YawMixFraction >  (MIN_YAWGAS / 2)) YawMixFraction = MIN_YAWGAS / 2;
-                if(YawMixFraction < -(MIN_YAWGAS / 2)) YawMixFraction = -(MIN_YAWGAS / 2);
+                CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
+        }
-        tmp_int = ParamSet.Gas_Max * STICK_GAIN;
+        tmp_int = ParamSet.GasMax * STICK_GAIN;
-    if(YawMixFraction >  ((tmp_int - GasMixFraction))) YawMixFraction =  ((tmp_int - GasMixFraction));
-    if(YawMixFraction < -((tmp_int - GasMixFraction))) YawMixFraction = -((tmp_int - GasMixFraction));
+        CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction));
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Axis
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    DiffNick = Reading_GyroNick - StickNick;    // get difference
+        DiffNick = PDPartNick - StickNick;      // get difference
-    if(Gyro_I_Factor) SumNick += IntegralNick * Gyro_I_Factor - StickNick; // I-part for attitude control
+        if(GyroIFactor) IPartNick += PPartNick - StickNick; // I-part for attitude control
-    else SumNick += DiffNick; // I-part for head holding
-    if(SumNick >  (STICK_GAIN * 16000L)) SumNick =  (STICK_GAIN * 16000L);
+        else IPartNick += DiffNick; // I-part for head holding
-    if(SumNick < -(STICK_GAIN * 16000L)) SumNick = -(STICK_GAIN * 16000L);
+        CHECK_MIN_MAX(IPartNick, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
-    pd_result = DiffNick + Ki * SumNick; // PI-controller for nick
-    tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction)/2)) / 64;
-    if(pd_result >  tmp_int) pd_result =  tmp_int;
-    if(pd_result < -tmp_int) pd_result = -tmp_int;
-        // Motor Front
-    MotorValue = GasMixFraction + pd_result + YawMixFraction;     // Mixer
-    MotorValue /= STICK_GAIN;
-        if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > ParamSet.Gas_Max)            MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
-        Motor_Front = MotorValue;
+        NickMixFraction = DiffNick + (IPartNick / Ki); // PID-controller for nick
- // Motor Rear
-        MotorValue = GasMixFraction - pd_result + YawMixFraction;     // Mixer
-        MotorValue /= STICK_GAIN;
-        if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > ParamSet.Gas_Max)      MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < ParamSet.Gas_Min)          MotorValue = ParamSet.Gas_Min;
         Motor_Rear = MotorValue;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Axis
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        DiffRoll = Reading_GyroRoll - StickRoll;        // get difference
+        DiffRoll = PDPartRoll - StickRoll;      // get difference
-    if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - StickRoll; // I-part for attitude control
+        if(GyroIFactor) IPartRoll += PPartRoll - StickRoll; // I-part for attitude control
-    else SumRoll += DiffRoll;  // I-part for head holding
-    if(SumRoll >  (STICK_GAIN * 16000L)) SumRoll =  (STICK_GAIN * 16000L);
+        else IPartRoll += DiffRoll;  // I-part for head holding
-    if(SumRoll < -(STICK_GAIN * 16000L)) SumRoll = -(STICK_GAIN * 16000L);
+        CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
+        RollMixFraction = DiffRoll + (IPartRoll / Ki);   // PID-controller for roll
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Limiter
-    pd_result = DiffRoll + Ki * SumRoll;         // PI-controller for roll
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction)/2)) / 64;
+        tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64;
-    if(pd_result >  tmp_int) pd_result =  tmp_int;
+        CHECK_MIN_MAX(NickMixFraction, -tmp_int, tmp_int);
+        CHECK_MIN_MAX(RollMixFraction, -tmp_int, tmp_int);
+#ifdef USE_QUADRO
+        // QuadroKopter Mixer
+        // Motor Front (++)
+    MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
+    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
+    MotorValue = MotorValue1 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor1 = MotorValue;
+        // Motor Rear (-+)
+        MotorValue = GasMixFraction - NickMixFraction + YawMixFraction;
+        MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
+        MotorValue = MotorValue2 / STICK_GAIN;
+        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor2 = MotorValue;
+        // Motor Right (--)
+        MotorValue = GasMixFraction - RollMixFraction - YawMixFraction;
+        MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
+        MotorValue = MotorValue3 / STICK_GAIN;
+        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    if(pd_result < -tmp_int) pd_result = -tmp_int;
+    Motor3 = MotorValue;
-    // Motor Left
+        // Motor Left (+-)
+    MotorValue = GasMixFraction + RollMixFraction - YawMixFraction;
-    MotorValue = GasMixFraction + pd_result - YawMixFraction;  // Mixer
+    MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
+    MotorValue = MotorValue4 / STICK_GAIN;
+        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor4 = MotorValue;
+#endif
+#ifdef USE_OCTO
+        // OctoKopter Mixer
+        // Motor 1 (+++)
+    MotorValue = GasMixFraction + NickMixFraction + RollMixFraction + YawMixFraction;
-    MotorValue /= STICK_GAIN;
+    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
-        if ((MotorValue < 0)) MotorValue = 0;
+    MotorValue = MotorValue1 / STICK_GAIN;
+        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor1= MotorValue;
+        // Motor 2 (+--)
+        MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction;
+        MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
+    MotorValue = MotorValue2 / STICK_GAIN;
+        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor2 = MotorValue;
+        // Motor 3 (+-+)
+        MotorValue = GasMixFraction + NickMixFraction - RollMixFraction + YawMixFraction;
+        MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
+    MotorValue = MotorValue3 / STICK_GAIN;
+        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor3 = MotorValue;
+        // Motor 4 (---)
+        MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction;
+        MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
+    MotorValue = MotorValue4 / STICK_GAIN;
+        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor4 = MotorValue;
+        // Motor 5 (--+)
+        MotorValue = GasMixFraction - NickMixFraction - RollMixFraction + YawMixFraction;
+        MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
+    MotorValue = MotorValue5 / STICK_GAIN;
+        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor5 = MotorValue;
+        // Motor 6 (-+-)
+        MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction;
+        MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
-        else if(MotorValue > ParamSet.Gas_Max)          MotorValue = ParamSet.Gas_Max;
+    MotorValue = MotorValue6 / STICK_GAIN;
-        if (MotorValue < ParamSet.Gas_Min)          MotorValue = ParamSet.Gas_Min;
+        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor_Left = MotorValue;
+    Motor6 = MotorValue;
+        // Motor7 (-++)
+        MotorValue = GasMixFraction - NickMixFraction + RollMixFraction + YawMixFraction;
+        MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
+    MotorValue = MotorValue7 / STICK_GAIN;
+        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor7 = MotorValue;
+    // Motor8 (++-)
+        MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction;
+        MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
+    MotorValue = MotorValue8 / STICK_GAIN;
+        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor8 = MotorValue;
+#endif
+#ifdef USE_OCTO2
+        // Octokopter Mixer alternativ setup
+    MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
+    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
+    MotorValue = MotorValue1 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor1 = MotorValue;
+    MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction;
+    MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
+    MotorValue = MotorValue2 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor2 = MotorValue;
+        MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
+    MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
+    MotorValue = MotorValue3 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor3 = MotorValue;
+        MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction;
+    MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
+    MotorValue = MotorValue4 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor4 = MotorValue;
+        MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
+    MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
+    MotorValue = MotorValue5 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor5 = MotorValue;
+        MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction;
+    MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
+    MotorValue = MotorValue6 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor6 = MotorValue;
+    MotorValue = GasMixFraction + RollMixFraction + YawMixFraction;
+    MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
+    MotorValue = MotorValue7 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor7 = MotorValue;
+    MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction;
+    MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
+    MotorValue = MotorValue8 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor8 = MotorValue;
+#endif
+#ifdef USE_OCTO3
+        // Octokopter Mixer alternativ setup
+    MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
+    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
+    MotorValue = MotorValue1 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor1 = MotorValue;
+    MotorValue = GasMixFraction + NickMixFraction - YawMixFraction;
+    MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
+    MotorValue = MotorValue2 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor2 = MotorValue;
+        MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
+    MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
+    MotorValue = MotorValue3 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor3 = MotorValue;
+        MotorValue = GasMixFraction - RollMixFraction - YawMixFraction;
+    MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
+    MotorValue = MotorValue4 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor4 = MotorValue;
+        MotorValue = GasMixFraction - NickMixFraction + YawMixFraction;
+    MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
+    MotorValue = MotorValue5 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor5 = MotorValue;
- // Motor Right
+        MotorValue = GasMixFraction - NickMixFraction - YawMixFraction;
-        MotorValue = GasMixFraction - pd_result - YawMixFraction;  // Mixer
+    MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
+    MotorValue = MotorValue6 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+        Motor6 = MotorValue;
+    MotorValue = GasMixFraction + RollMixFraction + YawMixFraction;
-        MotorValue /= STICK_GAIN;
+    MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
-        if ((MotorValue < 0)) MotorValue = 0;
+    MotorValue = MotorValue7 / STICK_GAIN;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor7 = MotorValue;
+    MotorValue = GasMixFraction + RollMixFraction - YawMixFraction;
+    MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
-        else if(MotorValue > ParamSet.Gas_Max)          MotorValue = ParamSet.Gas_Max;
+    MotorValue = MotorValue8 / STICK_GAIN;
-        if (MotorValue < ParamSet.Gas_Min)          MotorValue = ParamSet.Gas_Min;
+    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+    Motor8 = MotorValue;
+#endif
     Motor_Right = MotorValue;

Subversion Repositories FlightCtrl

(root)/branches/V0.72p Code Redesign killagreg/fc.c - Rev 1179 → 1180