WebSVN - FlightCtrl - Diff - Rev 908 and 911 - /branches/V0.69k Code Redesign killagreg/fc.c

 #endif
 #include "led.h"
 volatile uint16_t I2CTimeout = 100;
 // gyro readings
-volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw;
+volatile int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
 // gyro neutral readings
-volatile int16_t AdNeutralPitch = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
+volatile int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
-volatile int16_t StartNeutralRoll = 0, StartNeutralPitch = 0;
+volatile int16_t StartNeutralRoll = 0, StartNeutralNick = 0;
 // mean accelerations
-volatile int16_t Mean_AccPitch, Mean_AccRoll, Mean_AccTop;
+volatile int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
 // neutral acceleration readings
 volatile int16_t NeutralAccX=0, NeutralAccY=0;
 volatile float NeutralAccZ = 0;
 // attitude gyro integrals
-volatile int32_t IntegralPitch = 0,IntegralPitch2 = 0;
+volatile int32_t IntegralNick = 0,IntegralNick2 = 0;
 volatile int32_t IntegralRoll = 0,IntegralRoll2 = 0;
 volatile int32_t IntegralYaw = 0;
-volatile int32_t Reading_IntegralGyroPitch = 0, Reading_IntegralGyroPitch2 = 0;
+volatile int32_t Reading_IntegralGyroNick = 0, Reading_IntegralGyroNick2 = 0;
 volatile int32_t Reading_IntegralGyroRoll = 0,  Reading_IntegralGyroRoll2 = 0;
 volatile int32_t Reading_IntegralGyroYaw = 0;
-volatile int32_t MeanIntegralPitch;
+volatile int32_t MeanIntegralNick;
 volatile int32_t MeanIntegralRoll;
 // attitude acceleration integrals
-volatile int32_t IntegralAccPitch = 0, IntegralAccRoll = 0;
+volatile int32_t IntegralAccNick = 0, IntegralAccRoll = 0;
 uint16_t BadCompassHeading = 500;
 int32_t YawGyroHeading;
 int16_t YawGyroDrift;
-int16_t NaviAccPitch = 0, NaviAccRoll = 0, NaviCntAcc = 0;
+int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;
 // flags
 uint8_t MotorsOn = 0;
 uint8_t EmergencyLanding = 0;
 uint16_t Model_Is_Flying = 0;
-int32_t TurnOver180Pitch = 250000L, TurnOver180Roll = 250000L;
+int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
 float Gyro_P_Factor;
 float Gyro_I_Factor;
-volatile int16_t  DiffPitch, DiffRoll;
+volatile 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
 volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left;
 // stick values derived by rc channels readings
-int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0;
+int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0;
-int16_t GPS_Pitch = 0, GPS_Roll = 0;
+int16_t GPS_Nick = 0, GPS_Roll = 0;
 void SetNeutral(void)
+{
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
-    AdNeutralPitch = 0;
+    AdNeutralNick = 0;
         AdNeutralRoll = 0;
         AdNeutralYaw = 0;
     FCParam.Yaw_PosFeedback = 0;
     FCParam.Yaw_NegFeedback = 0;
     CalibMean();
         CalibMean();
     if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL))  // Height Control activated?
+    {
                 if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset();
+    }
-        AdNeutralPitch = AdValueGyrPitch;
+        AdNeutralNick = AdValueGyrNick;
         AdNeutralRoll  = AdValueGyrRoll;
         AdNeutralYaw   = AdValueGyrYaw;
         StartNeutralRoll  = AdNeutralRoll;
-        StartNeutralPitch = AdNeutralPitch;
+        StartNeutralNick = AdNeutralNick;
-    if(GetParamWord(PID_ACC_PITCH) > 1023)
+    if(GetParamWord(PID_ACC_NICK) > 1023)
+    {
                 NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY;
-                NeutralAccX = abs(Mean_AccPitch) / ACC_AMPLIFY;
+                NeutralAccX = abs(Mean_AccNick) / ACC_AMPLIFY;
                 NeutralAccZ = Current_AccZ;
+    }
     else
+    {
-                NeutralAccX = (int16_t)GetParamWord(PID_ACC_PITCH);
+                NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK);
             NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL);
             NeutralAccZ = (int16_t)GetParamWord(PID_ACC_Z);
+    }
-        Reading_IntegralGyroPitch = 0;
+        Reading_IntegralGyroNick = 0;
-    Reading_IntegralGyroPitch2 = 0;
+    Reading_IntegralGyroNick2 = 0;
     Reading_IntegralGyroRoll = 0;
     Reading_IntegralGyroRoll2 = 0;
     Reading_IntegralGyroYaw = 0;
-    Reading_GyroPitch = 0;
+    Reading_GyroNick = 0;
     Reading_GyroRoll = 0;
     Reading_GyroYaw = 0;
     StartAirPressure = AirPressure;
     HeightD = 0;
     Reading_Integral_Top = 0;
     CompassCourse = CompassHeading;
     BeepTime = 50;
-        TurnOver180Pitch = ((int32_t) ParamSet.AngleTurnOverPitch * 2500L) +15000L;
+        TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L;
         TurnOver180Roll =  ((int32_t) ParamSet.AngleTurnOverRoll *  2500L) +15000L;
     ExternHeightValue = 0;
-    GPS_Pitch = 0;
+    GPS_Nick = 0;
     GPS_Roll = 0;
     YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
     YawGyroDrift = 0;
+}
     static int32_t tmpl,tmpl2;
  // Get offset corrected gyro readings (~ to angular velocity)
     Reading_GyroYaw   = AdNeutralYaw    - AdValueGyrYaw;
     Reading_GyroRoll  = AdValueGyrRoll  - AdNeutralRoll;
-    Reading_GyroPitch = AdValueGyrPitch - AdNeutralPitch;
+    Reading_GyroNick = AdValueGyrNick - AdNeutralNick;
 // Acceleration Sensor
         // sliding average sensor readings
-        Mean_AccPitch = ((int32_t)Mean_AccPitch * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccPitch))) / 2L;
+        Mean_AccNick = ((int32_t)Mean_AccNick * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 2L;
         Mean_AccRoll  = ((int32_t)Mean_AccRoll * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 2L;
         Mean_AccTop   = ((int32_t)Mean_AccTop * 1 + ((int32_t)AdValueAccTop)) / 2L;
         // sum sensor readings for later averaging
-    IntegralAccPitch += ACC_AMPLIFY * AdValueAccPitch;
+    IntegralAccNick += ACC_AMPLIFY * AdValueAccNick;
     IntegralAccRoll  += ACC_AMPLIFY * AdValueAccRoll;
-    NaviAccPitch += AdValueAccPitch;
+    NaviAccNick += AdValueAccNick;
     if(YawGyroHeading >= (360L * YAW_GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * YAW_GYRO_DEG_FACTOR;  // 360° Wrap
         if(YawGyroHeading < 0)                             YawGyroHeading += 360L * YAW_GYRO_DEG_FACTOR;
         // Coupling fraction
-        if(!Looping_Pitch && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
+        if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
+        {
-                tmpl = (Reading_GyroYaw * Reading_IntegralGyroPitch) / 2048L;
+                tmpl = (Reading_GyroYaw * Reading_IntegralGyroNick) / 2048L;
                 tmpl *= FCParam.Yaw_PosFeedback;
                 tmpl /= 4096L;
                 tmpl2 = ( Reading_GyroYaw * Reading_IntegralGyroRoll) / 2048L;
         else
+        {
                 if(AdValueGyrRoll > 2020) Reading_GyroRoll = +1000;
                 if(AdValueGyrRoll > 2034) Reading_GyroRoll = +2000;
+        }
-// Pitch
+// Nick
-        Reading_GyroPitch -= tmpl2;
+        Reading_GyroNick -= tmpl2;
-        Reading_GyroPitch -= (tmpl*FCParam.Yaw_NegFeedback) / 512L;
+        Reading_GyroNick -= (tmpl*FCParam.Yaw_NegFeedback) / 512L;
-        Reading_IntegralGyroPitch2 += Reading_GyroPitch;
+        Reading_IntegralGyroNick2 += Reading_GyroNick;
-        Reading_IntegralGyroPitch  += Reading_GyroPitch - AttitudeCorrectionPitch;
+        Reading_IntegralGyroNick  += Reading_GyroNick - AttitudeCorrectionNick;
-        if(Reading_IntegralGyroPitch > TurnOver180Pitch)
+        if(Reading_IntegralGyroNick > TurnOver180Nick)
         {
-         Reading_IntegralGyroPitch = -(TurnOver180Pitch - 25000L);
+         Reading_IntegralGyroNick = -(TurnOver180Nick - 25000L);
-         Reading_IntegralGyroPitch2 = Reading_IntegralGyroPitch;
+         Reading_IntegralGyroNick2 = Reading_IntegralGyroNick;
+        }
-        if(Reading_IntegralGyroPitch < -TurnOver180Pitch)
+        if(Reading_IntegralGyroNick < -TurnOver180Nick)
+        {
-         Reading_IntegralGyroPitch =  (TurnOver180Pitch - 25000L);
+         Reading_IntegralGyroNick =  (TurnOver180Nick - 25000L);
-         Reading_IntegralGyroPitch2 = Reading_IntegralGyroPitch;
+         Reading_IntegralGyroNick2 = Reading_IntegralGyroNick;
+        }
-        if(AdValueGyrPitch < 15)   Reading_GyroPitch = -1000;
+        if(AdValueGyrNick < 15)   Reading_GyroNick = -1000;
-        if(AdValueGyrPitch <  7)   Reading_GyroPitch = -2000;
+        if(AdValueGyrNick <  7)   Reading_GyroNick = -2000;
         if(BoardRelease == 10)
+        {
-                if(AdValueGyrPitch > 1010) Reading_GyroPitch = +1000;
+                if(AdValueGyrNick > 1010) Reading_GyroNick = +1000;
-                if(AdValueGyrPitch > 1017) Reading_GyroPitch = +2000;
+                if(AdValueGyrNick > 1017) Reading_GyroNick = +2000;
+        }
         else
+        {
-                if(AdValueGyrPitch > 2020) Reading_GyroPitch = +1000;
+                if(AdValueGyrNick > 2020) Reading_GyroNick = +1000;
-                if(AdValueGyrPitch > 2034) Reading_GyroPitch = +2000;
+                if(AdValueGyrNick > 2034) Reading_GyroNick = +2000;
+        }
 // start ADC again to capture measurement values for the next loop
     ADC_Enable();
     IntegralYaw    = Reading_IntegralGyroYaw;
-    IntegralPitch  = Reading_IntegralGyroPitch;
+    IntegralNick  = Reading_IntegralGyroNick;
     IntegralRoll   = Reading_IntegralGyroRoll;
-    IntegralPitch2 = Reading_IntegralGyroPitch2;
+    IntegralNick2 = Reading_IntegralGyroNick2;
     IntegralRoll2  = Reading_IntegralGyroRoll2;
-        if((ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER) && !Looping_Pitch && !Looping_Roll)
+        if((ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER) && !Looping_Nick && !Looping_Roll)
+        {
-                if(Reading_GyroPitch > 200)       Reading_GyroPitch += 4 * (Reading_GyroPitch - 200);
+                if(Reading_GyroNick > 200)       Reading_GyroNick += 4 * (Reading_GyroNick - 200);
-                else if(Reading_GyroPitch < -200) Reading_GyroPitch += 4 * (Reading_GyroPitch + 200);
+                else if(Reading_GyroNick < -200) Reading_GyroNick += 4 * (Reading_GyroNick + 200);
                 if(Reading_GyroRoll > 200)        Reading_GyroRoll  += 4 * (Reading_GyroRoll - 200);
 void CalibMean(void)
+{
     // stop ADC to avoid changing values during calculation
         ADC_Disable();
-        Reading_GyroPitch = AdValueGyrPitch;
+        Reading_GyroNick = AdValueGyrNick;
         Reading_GyroRoll  = AdValueGyrRoll;
         Reading_GyroYaw   = AdValueGyrYaw;
-        Mean_AccPitch = ACC_AMPLIFY * (int32_t)AdValueAccPitch;
+        Mean_AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick;
         Mean_AccRoll  = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
         Mean_AccTop   = (int32_t)AdValueAccTop;
     // start ADC (enables internal trigger so that the ISR in analog.c
     // updates the readings once)
     ADC_Enable();
-        TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
+        TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L;
         TurnOver180Roll =  (int32_t) ParamSet.AngleTurnOverRoll  * 2500L;
                 CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4,0,255);
                 CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255);
                 CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255);
                 CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255);
                 CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255);
-                CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255);
+                CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl,0,255);
-                CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,0,255);
+                CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit,0,255);
                 CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255);
                 CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255);
                 CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255);
                 Ki = (float) FCParam.I_Factor * FACTOR_I;
+        }
 void SetCompassCalState(void)
+{
         static uint8_t stick = 1;
-    // if pitch is centered or top set stick to zero
+    // if nick is centered or top set stick to zero
-        if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -20) stick = 0;
+        if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -20) stick = 0;
-        // if pitch is down trigger to next cal state
+        // if nick is down trigger to next cal state
-        if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -70) && !stick)
+        if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick)
+        {
                 stick = 1;
                 CompassCalState++;
 /*  MotorControl                                                        */
 /************************************************************************/
 void MotorControl(void)
+{
         int16_t MotorValue, pd_result, h, tmp_int;
-        int16_t YawMixFraction, ThrustMixFraction;
+        int16_t YawMixFraction, GasMixFraction;
-        static int32_t SumPitch = 0, SumRoll = 0;
+        static int32_t SumNick = 0, SumRoll = 0;
         static int32_t SetPointYaw = 0;
-        static int32_t IntegralErrorPitch = 0;
+        static int32_t IntegralErrorNick = 0;
         static int32_t IntegralErrorRoll = 0;
         static uint16_t RcLostTimer;
         static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
         static uint8_t HeightControlActive = 0;
-        static int16_t HeightControlThrust = 0;
+        static int16_t HeightControlGas = 0;
         static int8_t TimerDebugOut = 0;
         static uint16_t UpdateCompassCourse = 0;
-        static int32_t CorrectionPitch, CorrectionRoll;
+        static int32_t CorrectionNick, CorrectionRoll;
         Mean();
         GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// determine thrust value
+// determine gas value
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        ThrustMixFraction = StickThrust;
+        GasMixFraction = StickGas;
-    if(ThrustMixFraction < ParamSet.Trust_Min + 10) ThrustMixFraction = ParamSet.Trust_Min + 10;
+    if(GasMixFraction < ParamSet.Gas_Min + 10) GasMixFraction = ParamSet.Gas_Min + 10;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // RC-signal is bad
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                   EmergencyLanding = 0; // emergency landing is over
+                }
                 ROT_ON; // set red led
                 if(Model_Is_Flying > 1000)  // wahrscheinlich in der Luft --> langsam absenken
+                {
-                        ThrustMixFraction = ParamSet.EmergencyThrust; // set emergency thrust
+                        GasMixFraction = ParamSet.EmergencyGas; // set emergency gas
                         EmergencyLanding = 1; // enable emergency landing
                         // set neutral rc inputs
-                        PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0;
+                        PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] = 0;
                         PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
                         PPM_diff[ParamSet.ChannelAssignment[CH_YAW]] = 0;
-                        PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0;
+                        PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0;
                         PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
                         PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0;
+                }
                 else MotorsOn = 0; // switch of all motors
         } // eof RC_Quality < 120
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(RC_Quality > 140)
+        {
                 EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay
                 // reset emergency timer
-                RcLostTimer = ParamSet.EmergencyThrustDuration * 50;
+                RcLostTimer = ParamSet.EmergencyGasDuration * 50;
-                if(ThrustMixFraction > 40)
+                if(GasMixFraction > 40)
+                {
                         if(Model_Is_Flying < 0xFFFF) Model_Is_Flying++;
+                }
                 if(Model_Is_Flying < 256)
+                {
-                        SumPitch = 0;
+                        SumNick = 0;
                         SumRoll = 0;
                         StickYaw = 0;
                         if(Model_Is_Flying == 250) UpdateCompassCourse = 1;
+                }
                 if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255;
                 if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
                 if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
                 if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
-                // if motors are off and the thrust stick is in the upper position
+                // if motors are off and the gas stick is in the upper position
-                if((PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 80) && MotorsOn == 0)
+                if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0)
+                {
                         // and if the yaw stick is in the leftmost position
                         if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // calibrate the neutral readings of all attitude sensors
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         {
-                                // thrust/yaw joystick is top left
+                                // gas/yaw joystick is top left
                                 //  _________
                                 // |x        |
                                 // |         |
                                 // |         |
                                 if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
+                                {
                                         delay_neutral = 0;
                                         GRN_OFF;
                                         Model_Is_Flying = 0;
-                                        // check roll/pitch stick position
+                                        // check roll/nick stick position
-                                        // if pitch stick is top or roll stick is left or right --> change parameter setting
+                                        // if nick stick is top or roll stick is left or right --> change parameter setting
-                                        // according to roll/pitch stick position
+                                        // according to roll/nick stick position
-                                        if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70)
+                                        if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70)
+                                        {
                                                  uint8_t setting = 1; // default
-                                                 // pitch/roll joystick
+                                                 // nick/roll joystick
                                                  //  _________
                                                  // |2   3   4|
                                                  // |         |
                                                  // |1       5|
                                                  // |         |
                                                  // |         |
                                                  //  ¯¯¯¯¯¯¯¯¯
-                                                 // roll stick leftmost and pitch stick centered --> setting 1
+                                                 // roll stick leftmost and nick stick centered --> setting 1
-                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 1;
+                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 1;
-                                                 // roll stick leftmost and pitch stick topmost --> setting 2
+                                                 // roll stick leftmost and nick stick topmost --> setting 2
-                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 2;
+                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 2;
-                                                 // roll stick centered an pitch stick topmost --> setting 3
+                                                 // roll stick centered an nick stick topmost --> setting 3
-                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 3;
+                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 3;
-                                                 // roll stick rightmost and pitch stick topmost --> setting 4
+                                                 // roll stick rightmost and nick stick topmost --> setting 4
-                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 4;
+                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 4;
-                                                 // roll stick rightmost and pitch stick centered --> setting 5
+                                                 // roll stick rightmost and nick stick centered --> setting 5
-                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) 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();
                                                  Beep(GetActiveParamSet());
+                                        }
                                         else
+                                        {
                                                 if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE))
+                                                {
-                                                        // if roll stick is centered and pitch stick is down
+                                                        // if roll stick is centered and nick stick is down
-                                                        if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 20 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -70)
+                                                        if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 20 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70)
+                                                        {
-                                                                // pitch/roll joystick
+                                                                // nick/roll joystick
                                                                 //  _________
                                                                 // |         |
                                                                 // |         |
                                                                 // |         |
                                                                 // |         |
                                                                 //  ¯¯¯¯¯¯¯¯¯
                                                                 // enable calibration state of compass
                                                                 CompassCalState = 1;
                                                                 BeepTime = 1000;
+                                                        }
-                                                        else // pitch and roll are centered
+                                                        else // nick and roll are centered
+                                                        {
                                                                 ParamSet_ReadFromEEProm(GetActiveParamSet());
                                                                 SetNeutral();
                                                                 Beep(GetActiveParamSet());
+                                                        }
+                                                }
-                                                else // pitch and roll are centered
+                                                else // nick and roll are centered
+                                                {
                                                         ParamSet_ReadFromEEProm(GetActiveParamSet());
                                                         SetNeutral();
                                                         Beep(GetActiveParamSet());
+                                                }
+                        {
                                 if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
+                                {
                                         delay_neutral = 0;
                                         GRN_OFF;
-                                        SetParamWord(PID_ACC_PITCH, 0xFFFF); // make value invalid
+                                        SetParamWord(PID_ACC_NICK, 0xFFFF); // make value invalid
                                         Model_Is_Flying = 0;
                                         SetNeutral();
                                         // Save ACC neutral settings to eeprom
-                                        SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX);
+                                        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;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// thrust stick is down
+// gas stick is down
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                if(PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < -85)
+                if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // and yaw stick is rightmost --> start motors
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
                                         delay_startmotors = 200; // do not repeat if once executed
                                         Model_Is_Flying = 1;
                                         MotorsOn = 1;
                                         SetPointYaw = 0;
                                         Reading_IntegralGyroYaw = 0;
-                                        Reading_IntegralGyroPitch = 0;
+                                        Reading_IntegralGyroNick = 0;
                                         Reading_IntegralGyroRoll = 0;
-                                        Reading_IntegralGyroPitch2 = IntegralPitch;
+                                        Reading_IntegralGyroNick2 = IntegralNick;
                                         Reading_IntegralGyroRoll2 = IntegralRoll;
-                                        SumPitch = 0;
+                                        SumNick = 0;
                                         SumRoll = 0;
                                         #if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
                                         if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
+                                        {
                                                 GPS_SetHomePosition();
         if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC
+        {
                 int tmp_int;
                 ParameterMapping(); // remapping params (online poti replacement)
                 // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
-                StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4;
+                StickNick = (StickNick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_P) / 4;
-                StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D;
+                StickNick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_D;
-                StickPitch -= (GPS_Pitch);
+                StickNick -= (GPS_Nick);
                 StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
                 StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
                 StickRoll -= (GPS_Roll);
-                // direct mapping of yaw and thrust
+                // direct mapping of yaw and gas
                 StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
-                StickThrust  = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] + 120;// shift to positive numbers
+                StickGas  = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers
                 #define KEY_VALUE (FCParam.UserParam8 * 4) // step width
                 if(DubWiseKeys[1]) BeepTime = 10;
                 if(DubWiseKeys[1] & DUB_KEY_UP)  tmp_int = KEY_VALUE;
                 else if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;
                 else tmp_int = 0;
-                ExternStickPitch = (ExternStickPitch * 7 + tmp_int) / 8;
+                ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
                 if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE;
                 else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE;
                 else tmp_int = 0;
                 ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
                 if(DubWiseKeys[0] & 8)  ExternStickYaw = 50;else
                 if(DubWiseKeys[0] & 4)  ExternStickYaw =-50;else ExternStickYaw = 0;
                 if(DubWiseKeys[0] & 2)  ExternHeightValue++;
                 if(DubWiseKeys[0] & 16) ExternHeightValue--;
-                StickPitch += (STICK_GAIN * ExternStickPitch) / 8;
+                StickNick += (STICK_GAIN * ExternStickNick) / 8;
                 StickRoll  += (STICK_GAIN * ExternStickRoll) / 8;
                 StickYaw   += (STICK_GAIN * ExternStickYaw);
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Analog control via serial communication
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(ExternControl.Config & 0x01 && FCParam.UserParam8 > 128)
+                {
-                         StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P;
+                         StickNick += (int16_t) ExternControl.Nick * (int16_t) ParamSet.Stick_P;
                          StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P;
                          StickYaw += ExternControl.Yaw;
                          ExternHeightValue =  (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain;
-                         if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust;
+                         if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
+                }
-                if(StickThrust < 0) StickThrust = 0;
+                if(StickGas < 0) StickGas = 0;
                 // disable I part of gyro control feedback
                 if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor =  0;
                 // avoid negative scaling factors
                 if(Gyro_P_Factor < 0) Gyro_P_Factor = 0;
                 if(Gyro_I_Factor < 0) Gyro_I_Factor = 0;
+                        {
                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0;
+                        }
+                }
-                if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1;
+                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1;
                 else
+                {
                         if(Looping_Top)  // Hysteresis
                         {
-                                if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0;
+                                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0;
+                        }
                 }
-                if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1;
+                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1;
                 else
+                {
                         if(Looping_Down) // Hysteresis
                         {
-                                if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0;
+                                if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0;
+                        }
+                }
                 if(Looping_Left || Looping_Right)   Looping_Roll = 1; else Looping_Roll = 0;
-                if(Looping_Top  || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0;
+                if(Looping_Top  || Looping_Down) {Looping_Nick = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Nick = 0;
         } // End of new RC-Values or Emergency Landing
         if(Looping_Roll) BeepTime = 100;
-        if(Looping_Roll || Looping_Pitch)
+        if(Looping_Roll || Looping_Nick)
+        {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         // set all inputs to save values
         if(EmergencyLanding)
+        {
                 StickYaw = 0;
-                StickPitch = 0;
+                StickNick = 0;
                 StickRoll = 0;
                 Gyro_P_Factor  = (float) 100 / (256.0 / STICK_GAIN);
                 Gyro_I_Factor = (float) 120 / (44000 / STICK_GAIN);
                 Looping_Roll = 0;
-                Looping_Pitch = 0;
+                Looping_Nick = 0;
-                MaxStickPitch = 0;
+                MaxStickNick = 0;
                 MaxStickRoll = 0;
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Trim Gyro-Integrals to ACC-Signals
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         #define BALANCE_NUMBER 256L
         // sum for averaging
-        MeanIntegralPitch  += IntegralPitch;
+        MeanIntegralNick  += IntegralNick;
         MeanIntegralRoll  += IntegralRoll;
-        if(Looping_Pitch || Looping_Roll) // if looping in any direction
+        if(Looping_Nick || Looping_Roll) // if looping in any direction
         {
                 // reset averaging for acc and gyro integral as well as gyro integral acc correction
                 MeasurementCounter = 0;
-                IntegralAccPitch = 0;
+                IntegralAccNick = 0;
                 IntegralAccRoll = 0;
-                MeanIntegralPitch = 0;
+                MeanIntegralNick = 0;
                 MeanIntegralRoll = 0;
-                Reading_IntegralGyroPitch2 = Reading_IntegralGyroPitch;
+                Reading_IntegralGyroNick2 = Reading_IntegralGyroNick;
                 Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll;
-                AttitudeCorrectionPitch = 0;
+                AttitudeCorrectionNick = 0;
                 AttitudeCorrectionRoll = 0;
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        if(!Looping_Pitch && !Looping_Roll) // if not lopping in any direction
+        if(!Looping_Nick && !Looping_Roll) // if not lopping in any direction
+        {
                 int32_t tmp_long, tmp_long2;
                 // determine the deviation of gyro integral from averaged acceleration sensor
-                tmp_long   =  (int32_t)(IntegralPitch / ParamSet.GyroAccFactor - (int32_t)Mean_AccPitch);
+                tmp_long   =  (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
                 tmp_long  /= 16;
                 if(tmp_long >  BALANCE)  tmp_long  = BALANCE;
                 if(tmp_long < -BALANCE)  tmp_long  =-BALANCE;
                 if(tmp_long2 > BALANCE)  tmp_long2 = BALANCE;
                 if(tmp_long2 <-BALANCE)  tmp_long2 =-BALANCE;
                 // correct current readings
-                Reading_IntegralGyroPitch -= tmp_long;
+                Reading_IntegralGyroNick -= tmp_long;
                 Reading_IntegralGyroRoll -= 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 MeanIntegralPitch_old, MeanIntegralRoll_old;
+                static int32_t MeanIntegralNick_old, MeanIntegralRoll_old;
                 // if not lopping in any direction (this should be alwais the case,
                 // because the Measurement counter is reset to 0 if looping in any direction is active.)
-                if(!Looping_Pitch && !Looping_Roll && !FunnelCourse)
+                if(!Looping_Nick && !Looping_Roll && !FunnelCourse)
+                {
                         // Calculate mean value of the gyro integrals
-                        MeanIntegralPitch /= BALANCE_NUMBER;
+                        MeanIntegralNick /= BALANCE_NUMBER;
                         MeanIntegralRoll  /= BALANCE_NUMBER;
                         // Calculate mean of the acceleration values
-                        IntegralAccPitch = (ParamSet.GyroAccFactor * IntegralAccPitch) / BALANCE_NUMBER;
+                        IntegralAccNick = (ParamSet.GyroAccFactor * IntegralAccNick) / BALANCE_NUMBER;
                         IntegralAccRoll  = (ParamSet.GyroAccFactor * IntegralAccRoll ) / BALANCE_NUMBER;
-                        // Pitch ++++++++++++++++++++++++++++++++++++++++++++++++
+                        // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
                         // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
-                        IntegralErrorPitch = (int32_t)(MeanIntegralPitch - (int32_t)IntegralAccPitch);
+                        IntegralErrorNick = (int32_t)(MeanIntegralNick - (int32_t)IntegralAccNick);
-                        CorrectionPitch = IntegralErrorPitch / ParamSet.GyroAccTrim;
+                        CorrectionNick = IntegralErrorNick / ParamSet.GyroAccTrim;
-                        AttitudeCorrectionPitch = CorrectionPitch / BALANCE_NUMBER;
+                        AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER;
                         // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
                         // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
                         IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
                         CorrectionRoll  = IntegralErrorRoll / ParamSet.GyroAccTrim;
                         AttitudeCorrectionRoll  = CorrectionRoll  / BALANCE_NUMBER;
-                        if((MaxStickPitch > 32) || (MaxStickRoll > 32) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25))
+                        if((MaxStickNick > 32) || (MaxStickRoll > 32) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25))
+                        {
-                                AttitudeCorrectionPitch /= 2;
+                                AttitudeCorrectionNick /= 2;
                                 AttitudeCorrectionRoll /= 2;
                         }
         // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         // Gyro-Drift ermitteln
         // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                        // deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
+                        // deviation of gyro nick integral (IntegralNick is corrected by averaged acc sensor)
-                        IntegralErrorPitch  = IntegralPitch2 - IntegralPitch;
+                        IntegralErrorNick  = IntegralNick2 - IntegralNick;
-                        Reading_IntegralGyroPitch2 -= IntegralErrorPitch;
+                        Reading_IntegralGyroNick2 -= IntegralErrorNick;
-                        // deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
+                        // deviation of gyro nick integral (IntegralNick is corrected by averaged acc sensor)
                         IntegralErrorRoll = IntegralRoll2 - IntegralRoll;
                         Reading_IntegralGyroRoll2 -= IntegralErrorRoll;
                         if(YawGyroDrift >  BALANCE_NUMBER/2) AdNeutralYaw++;
                         if(YawGyroDrift < -BALANCE_NUMBER/2) AdNeutralYaw--;
                         YawGyroDrift = 0;
 /*
-                        DebugOut.Analog[17] = IntegralAccPitch / 26;
+                        DebugOut.Analog[17] = IntegralAccNick / 26;
                         DebugOut.Analog[18] = IntegralAccRoll / 26;
-                        DebugOut.Analog[19] = IntegralErrorPitch;// / 26;
+                        DebugOut.Analog[19] = IntegralErrorNick;// / 26;
                         DebugOut.Analog[20] = IntegralErrorRoll;// / 26;
-                        DebugOut.Analog[21] = MeanIntegralPitch / 26;
+                        DebugOut.Analog[21] = MeanIntegralNick / 26;
                         DebugOut.Analog[22] = MeanIntegralRoll / 26;
-                        //DebugOut.Analog[28] = CorrectionPitch;
+                        //DebugOut.Analog[28] = CorrectionNick;
                         DebugOut.Analog[29] = CorrectionRoll;
                         DebugOut.Analog[30] = AttitudeCorrectionRoll * 10;
 */
                         #define ERROR_LIMIT  (BALANCE_NUMBER * 4)
                         #define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
                         #define MOVEMENT_LIMIT 20000
-        // Pitch +++++++++++++++++++++++++++++++++++++++++++++++++
+        // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
-                        cnt = 1;// + labs(IntegralErrorPitch) / 4096;
+                        cnt = 1;// + labs(IntegralErrorNick) / 4096;
-                        CorrectionPitch = 0;
+                        CorrectionNick = 0;
-                        if(labs(MeanIntegralPitch_old - MeanIntegralPitch) < MOVEMENT_LIMIT)
+                        if(labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT)
                         {
-                                if(IntegralErrorPitch >  ERROR_LIMIT2)
+                                if(IntegralErrorNick >  ERROR_LIMIT2)
+                                {
                                         if(last_n_p)
+                                        {
-                                                cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2;
+                                                cnt += labs(IntegralErrorNick) / ERROR_LIMIT2;
-                                                CorrectionPitch = IntegralErrorPitch / 8;
+                                                CorrectionNick = IntegralErrorNick / 8;
-                                                if(CorrectionPitch > 5000) CorrectionPitch = 5000;
+                                                if(CorrectionNick > 5000) CorrectionNick = 5000;
-                                                AttitudeCorrectionPitch += CorrectionPitch / BALANCE_NUMBER;
+                                                AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
+                                        }
                                         else last_n_p = 1;
                                 }
                                 else  last_n_p = 0;
-                                if(IntegralErrorPitch < -ERROR_LIMIT2)
+                                if(IntegralErrorNick < -ERROR_LIMIT2)
+                                {
                                         if(last_n_n)
+                                        {
-                                                cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2;
+                                                cnt += labs(IntegralErrorNick) / ERROR_LIMIT2;
-                                                CorrectionPitch = IntegralErrorPitch / 8;
+                                                CorrectionNick = IntegralErrorNick / 8;
                                 cnt = 0;
                                 BadCompassHeading = 500;
+                        }
                         if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
                         // correct Gyro Offsets
-                        if(IntegralErrorPitch >  ERROR_LIMIT)   AdNeutralPitch += cnt;
+                        if(IntegralErrorNick >  ERROR_LIMIT)   AdNeutralNick += cnt;
-                        if(IntegralErrorPitch < -ERROR_LIMIT)   AdNeutralPitch -= cnt;
+                        if(IntegralErrorNick < -ERROR_LIMIT)   AdNeutralNick -= cnt;
         // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
-                        cnt = 1;// + labs(IntegralErrorPitch) / 4096;
+                        cnt = 1;// + labs(IntegralErrorNick) / 4096;
                         CorrectionRoll = 0;
                         if(labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT)
+                        {
                                 if(IntegralErrorRoll >  ERROR_LIMIT2)
                         if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
                         if(IntegralErrorRoll >  ERROR_LIMIT)   AdNeutralRoll += cnt;
                         if(IntegralErrorRoll < -ERROR_LIMIT)   AdNeutralRoll -= cnt;
 /*
                         DebugOut.Analog[27] = CorrectionRoll;
-                        DebugOut.Analog[23] = AdNeutralPitch;//10*(AdNeutralPitch - StartNeutralPitch);
+                        DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
                         DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
 */
+                }
                 else // looping is active
+                {
                         AttitudeCorrectionRoll  = 0;
-                        AttitudeCorrectionPitch = 0;
+                        AttitudeCorrectionNick = 0;
                         FunnelCourse = 0;
+                }
                 // if Gyro_I_Factor == 0 , for example at Heading Hold, ignore attitude correction
                 if(!Gyro_I_Factor)
+                {
                         AttitudeCorrectionRoll  = 0;
-                        AttitudeCorrectionPitch = 0;
+                        AttitudeCorrectionNick = 0;
+                }
         // +++++++++++++++++++++++++++++++++++++++++++++++++++++
-                MeanIntegralPitch_old = MeanIntegralPitch;
+                MeanIntegralNick_old = MeanIntegralNick;
                 MeanIntegralRoll_old  = MeanIntegralRoll;
         // +++++++++++++++++++++++++++++++++++++++++++++++++++++
                 // reset variables used for averaging
-                IntegralAccPitch = 0;
+                IntegralAccNick = 0;
                 IntegralAccRoll = 0;
-                MeanIntegralPitch = 0;
+                MeanIntegralNick = 0;
                 MeanIntegralRoll = 0;
                 MeasurementCounter = 0;
                                 MM3_Heading();
+                        }
                         #endif
                         // get maximum attitude angle
-                        w = abs(IntegralPitch/512);
+                        w = abs(IntegralNick/512);
                         v = abs(IntegralRoll /512);
                         if(v > w) w = v;
                         // update compass course
                         if (w < 25 && UpdateCompassCourse && !BadCompassHeading)
                                         BadCompassHeading--;
+                                }
                                 else
                                 {   //
                                         YawGyroDrift += error;
-                                        v = 64 + (MaxStickPitch + MaxStickRoll) / 8;
+                                        v = 64 + (MaxStickNick + MaxStickRoll) / 8;
                                         // calc course deviation
                                         r = ((540 + (YawGyroHeading / YAW_GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
                                         v = (r * w) / v; // align to compass course
                                         // limit yaw rate
                                         w = 3 * FCParam.CompassYawEffect;
                 if(EmergencyLanding) GPS_Main(230); // enables Comming Home
                 else GPS_Main(Poti3);               // behavior controlled by Poti3
+        }
         else
+        {
-                GPS_Pitch = 0;
+                GPS_Nick = 0;
                 GPS_Roll = 0;
+        }
         #endif
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(!TimerDebugOut--)
+        {
                 TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz)
-                DebugOut.Analog[0]  = IntegralPitch / ParamSet.GyroAccFactor;
+                DebugOut.Analog[0]  = IntegralNick / ParamSet.GyroAccFactor;
                 DebugOut.Analog[1]  = IntegralRoll / ParamSet.GyroAccFactor;
-                DebugOut.Analog[2]  = Mean_AccPitch;
+                DebugOut.Analog[2]  = Mean_AccNick;
                 DebugOut.Analog[3]  = Mean_AccRoll;
                 DebugOut.Analog[4]  = Reading_GyroYaw;
                 DebugOut.Analog[5]  = ReadingHeight;
                 DebugOut.Analog[6]  = (Reading_Integral_Top / 512);
                 DebugOut.Analog[20] = ServoValue;
-                DebugOut.Analog[30] = GPS_Pitch;
+                DebugOut.Analog[30] = GPS_Nick;
                 DebugOut.Analog[22] = motor_rx[5];
                 DebugOut.Analog[23] = motor_rx[6];
                 DebugOut.Analog[24] = motor_rx[7];
                 DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
-                DebugOut.Analog[9]  = Reading_GyroPitch;
+                DebugOut.Analog[9]  = Reading_GyroNick;
                 DebugOut.Analog[9]  = SetPointHeight;
                 DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128;
                 DebugOut.Analog[10] = FCParam.Gyro_I;
                 DebugOut.Analog[10] = ParamSet.Gyro_I;
                 DebugOut.Analog[9]  = CompassOffCourse;
-                DebugOut.Analog[10] = ThrustMixFraction;
+                DebugOut.Analog[10] = GasMixFraction;
                 DebugOut.Analog[3]  = HeightD * 32;
-                DebugOut.Analog[4]  = HeightControlThrust;
+                DebugOut.Analog[4]  = HeightControlGas;
                 */
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  calculate control feedback from angle (gyro integral) and agular velocity (gyro signal)
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        if(Looping_Pitch) Reading_GyroPitch = Reading_GyroPitch * Gyro_P_Factor;
+        if(Looping_Nick) Reading_GyroNick = Reading_GyroNick * Gyro_P_Factor;
-        else Reading_GyroPitch = IntegralPitch * Gyro_I_Factor + Reading_GyroPitch * Gyro_P_Factor;
+        else Reading_GyroNick = IntegralNick * Gyro_I_Factor + Reading_GyroNick * Gyro_P_Factor;
         if(Looping_Roll) Reading_GyroRoll = Reading_GyroRoll * Gyro_P_Factor;
         else Reading_GyroRoll = IntegralRoll * Gyro_I_Factor + Reading_GyroRoll * Gyro_P_Factor;
         Reading_GyroYaw = Reading_GyroYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2;
-        DebugOut.Analog[21] = Reading_GyroPitch;
+        DebugOut.Analog[21] = Reading_GyroNick;
         DebugOut.Analog[22] = Reading_GyroRoll;
         // limit control feedback
         #define MAX_SENSOR  (4096 * STICK_GAIN)
-        if(Reading_GyroPitch >  MAX_SENSOR) Reading_GyroPitch =  MAX_SENSOR;
+        if(Reading_GyroNick >  MAX_SENSOR) Reading_GyroNick =  MAX_SENSOR;
-        if(Reading_GyroPitch < -MAX_SENSOR) Reading_GyroPitch = -MAX_SENSOR;
+        if(Reading_GyroNick < -MAX_SENSOR) Reading_GyroNick = -MAX_SENSOR;
-        if(Reading_GyroRoll  >  MAX_SENSOR) Reading_GyroRoll  =  MAX_SENSOR;
+        if(Reading_GyroRoll >  MAX_SENSOR) Reading_GyroRoll =  MAX_SENSOR;
-        if(Reading_GyroRoll  < -MAX_SENSOR) Reading_GyroRoll  = -MAX_SENSOR;
+        if(Reading_GyroRoll < -MAX_SENSOR) Reading_GyroRoll = -MAX_SENSOR;
-        if(Reading_GyroYaw   >  MAX_SENSOR) Reading_GyroYaw   =  MAX_SENSOR;
+        if(Reading_GyroYaw  >  MAX_SENSOR) Reading_GyroYaw  =  MAX_SENSOR;
-        if(Reading_GyroYaw   < -MAX_SENSOR) Reading_GyroYaw   = -MAX_SENSOR;
+        if(Reading_GyroYaw  < -MAX_SENSOR) Reading_GyroYaw  = -MAX_SENSOR;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Height Control
                         SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
                         HeightControlActive = 1;
+                }
                 // get current height
                 h = ReadingHeight;
-                // if current height is above the setpoint reduce thrust
+                // if current height is above the setpoint reduce gas
                 if((h > SetPointHeight) && HeightControlActive)
+                {
-                        // ThrustMixFraction - HightDeviation * P  - HeightChange * D - ACCTop * DACC
+                        // GasMixFraction - HightDeviation * P  - HeightChange * D - ACCTop * DACC
                         // height difference -> P control part
                         h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / (16 / STICK_GAIN);
-                        h = ThrustMixFraction - h; // reduce gas
+                        h = GasMixFraction - h; // reduce gas
                         // height gradient --> D control part
                         //h -= (HeightD * FCParam.Height_D) / (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);
                         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 thrust
+                        // update height control gas
-                        HeightControlThrust = (HeightControlThrust*15 + h) / 16;
+                        HeightControlGas = (HeightControlGas*15 + h) / 16;
-                        // limit thrust reduction
+                        // limit gas reduction
-                        if(HeightControlThrust < ParamSet.Height_MinThrust * STICK_GAIN)
+                        if(HeightControlGas < ParamSet.Height_MinGas * STICK_GAIN)
+                        {
-                                if(ThrustMixFraction >= ParamSet.Height_MinThrust * STICK_GAIN) HeightControlThrust = ParamSet.Height_MinThrust * STICK_GAIN;
+                                if(GasMixFraction >= ParamSet.Height_MinGas * STICK_GAIN) HeightControlGas = ParamSet.Height_MinGas * STICK_GAIN;
-                                // allows landing also if thrust stick is reduced below min thrust on height control
+                                // allows landing also if gas stick is reduced below min gas on height control
-                                if(ThrustMixFraction < ParamSet.Height_MinThrust * STICK_GAIN) HeightControlThrust = ThrustMixFraction;
+                                if(GasMixFraction < ParamSet.Height_MinGas * STICK_GAIN) HeightControlGas = GasMixFraction;
+                        }
-                        // limit thrust to stick setting
+                        // limit gas to stick setting
-                        if(HeightControlThrust > ThrustMixFraction) HeightControlThrust = ThrustMixFraction;
+                        if(HeightControlGas > GasMixFraction) HeightControlGas = GasMixFraction;
-                        ThrustMixFraction = HeightControlThrust;
+                        GasMixFraction = HeightControlGas;
+                }
+        }
-        // limit thrust to parameter setting
+        // limit gas to parameter setting
-        if(ThrustMixFraction > (ParamSet.Trust_Max - 20) * STICK_GAIN) ThrustMixFraction = (ParamSet.Trust_Max - 20) * STICK_GAIN;
+        if(GasMixFraction > (ParamSet.Gas_Max - 20) * STICK_GAIN) GasMixFraction = (ParamSet.Gas_Max - 20) * STICK_GAIN;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mixer and PI-Controller
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        DebugOut.Analog[7] = ThrustMixFraction;
+        DebugOut.Analog[7] = GasMixFraction;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Yaw-Fraction
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     YawMixFraction = Reading_GyroYaw - SetPointYaw * STICK_GAIN;     // yaw controller
-        #define MIN_YAWTHRUST (40 * STICK_GAIN)  // yaw also below this thrust value
+        #define MIN_YAWGAS (40 * STICK_GAIN)  // yaw also below this gas value
         // limit YawMixFraction
-        if(ThrustMixFraction > MIN_YAWTHRUST)
+        if(GasMixFraction > MIN_YAWGAS)
+        {
-                if(YawMixFraction >  (ThrustMixFraction / 2)) YawMixFraction = ThrustMixFraction / 2;
+                if(YawMixFraction >  (GasMixFraction / 2)) YawMixFraction = GasMixFraction / 2;
-                if(YawMixFraction < -(ThrustMixFraction / 2)) YawMixFraction = -(ThrustMixFraction / 2);
+                if(YawMixFraction < -(GasMixFraction / 2)) YawMixFraction = -(GasMixFraction / 2);
+        }
         else
+        {
-                if(YawMixFraction >  (MIN_YAWTHRUST / 2)) YawMixFraction = MIN_YAWTHRUST / 2;
+                if(YawMixFraction >  (MIN_YAWGAS / 2)) YawMixFraction = MIN_YAWGAS / 2;
-                if(YawMixFraction < -(MIN_YAWTHRUST / 2)) YawMixFraction = -(MIN_YAWTHRUST / 2);
+                if(YawMixFraction < -(MIN_YAWGAS / 2)) YawMixFraction = -(MIN_YAWGAS / 2);
+        }
-        tmp_int = ParamSet.Trust_Max * STICK_GAIN;
+        tmp_int = ParamSet.Gas_Max * STICK_GAIN;
-    if(YawMixFraction >  ((tmp_int - ThrustMixFraction))) YawMixFraction =  ((tmp_int - ThrustMixFraction));
+    if(YawMixFraction >  ((tmp_int - GasMixFraction))) YawMixFraction =  ((tmp_int - GasMixFraction));
-    if(YawMixFraction < -((tmp_int - ThrustMixFraction))) YawMixFraction = -((tmp_int - ThrustMixFraction));
+    if(YawMixFraction < -((tmp_int - GasMixFraction))) YawMixFraction = -((tmp_int - GasMixFraction));
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Pitch-Axis
+// Nick-Axis
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    DiffPitch = Reading_GyroPitch - StickPitch; // get difference
+    DiffNick = Reading_GyroNick - StickNick;    // get difference
-    if(Gyro_I_Factor) SumPitch += IntegralPitch * Gyro_I_Factor - StickPitch; // I-part for attitude control
+    if(Gyro_I_Factor) SumNick += IntegralNick * Gyro_I_Factor - StickNick; // I-part for attitude control
-    else SumPitch += DiffPitch; // I-part for head holding
+    else SumNick += DiffNick; // I-part for head holding
-    if(SumPitch >  (STICK_GAIN * 16000L)) SumPitch =  (STICK_GAIN * 16000L);
+    if(SumNick >  (STICK_GAIN * 16000L)) SumNick =  (STICK_GAIN * 16000L);
-    if(SumPitch < -(STICK_GAIN * 16000L)) SumPitch = -(STICK_GAIN * 16000L);
+    if(SumNick < -(STICK_GAIN * 16000L)) SumNick = -(STICK_GAIN * 16000L);
-    pd_result = DiffPitch + Ki * SumPitch; // PI-controller for pitch
+    pd_result = DiffNick + Ki * SumNick; // PI-controller for nick
-    tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + 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;
     if(pd_result < -tmp_int) pd_result = -tmp_int;
         // Motor Front
-    MotorValue = ThrustMixFraction + pd_result + YawMixFraction;          // Mixer
+    MotorValue = GasMixFraction + pd_result + YawMixFraction;     // Mixer
     MotorValue /= STICK_GAIN;
         if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > ParamSet.Trust_Max)            MotorValue = ParamSet.Trust_Max;
+        else if(MotorValue > ParamSet.Gas_Max)              MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
+        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
         Motor_Front = MotorValue;
  // Motor Rear
-        MotorValue = ThrustMixFraction - pd_result + YawMixFraction;     // Mixer
+        MotorValue = GasMixFraction - pd_result + YawMixFraction;     // Mixer
         MotorValue /= STICK_GAIN;
         if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > ParamSet.Trust_Max)            MotorValue = ParamSet.Trust_Max;
+        else if(MotorValue > ParamSet.Gas_Max)      MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
+        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
         Motor_Rear = MotorValue;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Axis
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = Reading_GyroRoll - StickRoll;        // get difference
     if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - StickRoll; // I-part for attitude control
     else SumRoll += DiffRoll;  // I-part for head holding
     if(SumRoll >  (STICK_GAIN * 16000L)) SumRoll =  (STICK_GAIN * 16000L);
     if(SumRoll < -(STICK_GAIN * 16000L)) SumRoll = -(STICK_GAIN * 16000L);
     pd_result = DiffRoll + Ki * SumRoll;         // PI-controller for roll
-    tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + 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;
     if(pd_result < -tmp_int) pd_result = -tmp_int;
     // Motor Left
-    MotorValue = ThrustMixFraction + pd_result - YawMixFraction;  // Mixer
+    MotorValue = GasMixFraction + pd_result - YawMixFraction;  // Mixer
     MotorValue /= STICK_GAIN;
         if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > ParamSet.Trust_Max)                MotorValue = ParamSet.Trust_Max;
+        else if(MotorValue > ParamSet.Gas_Max)          MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
+        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
     Motor_Left = MotorValue;
  // Motor Right
-        MotorValue = ThrustMixFraction - pd_result - YawMixFraction;  // Mixer
+        MotorValue = GasMixFraction - pd_result - YawMixFraction;  // Mixer

Subversion Repositories FlightCtrl

(root)/branches/V0.69k Code Redesign killagreg/fc.c @ 912 - Rev 908 → 911