WebSVN - FlightCtrl - Diff - Rev 701 and 703 - /branches/V0.68d Code Redesign killagreg/fc.c

 #include "uart.h"
 #include "rc.h"
 #include "twimaster.h"
 #include "mm3.h"
 unsigned char h,m,s;
+volatile unsigned int I2CTimeout = 100;
-volatile unsigned int I2CTimeout = 100;
+// gyro readings
+volatile int16_t ReadingPitch, ReadingRoll, ReadingYaw;
-volatile int ReadingPitch, ReadingRoll, ReadingYaw;
+// gyro neutral readings
+volatile int16_t AdNeutralPitch = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
+volatile int16_t StartNeutralRoll = 0, StartNeutralPitch = 0;
-volatile int AdNeutralPitch = 0,AdNeutralRoll = 0,AdNeutralYaw = 0, StartNeutralRoll = 0, StartNeutralPitch = 0;
+// mean accelerations
+volatile int16_t Mean_AccPitch, Mean_AccRoll, Mean_AccTop;
+// neutral acceleration readings
-volatile int Mean_AccPitch, Mean_AccRoll, Mean_AccTop, NeutralAccX=0, NeutralAccY=0;
+volatile int16_t NeutralAccX=0, NeutralAccY=0;
+volatile float NeutralAccZ = 0;
-volatile float NeutralAccZ = 0;
-volatile long IntegralPitch = 0,IntegralPitch2 = 0;
+// attitude gyro integrals
-volatile long IntegralRoll = 0,IntegralRoll2 = 0;
+volatile int32_t IntegralPitch = 0,IntegralPitch2 = 0;
-volatile long IntegralAccPitch = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
+volatile int32_t IntegralRoll = 0,IntegralRoll2 = 0;
-volatile long Integral_Yaw = 0;
+volatile int32_t IntegralYaw = 0;
-volatile long Reading_IntegralPitch = 0,Reading_IntegralPitch2 = 0;
+volatile int32_t Reading_IntegralPitch = 0, Reading_IntegralPitch2 = 0;
-volatile long Reading_IntegralRoll = 0,Reading_IntegralRoll2 = 0;
+volatile int32_t Reading_IntegralRoll = 0,  Reading_IntegralRoll2 = 0;
+volatile int32_t Reading_IntegralYaw = 0,   Reading_IntegralYaw2 = 0;
-volatile long Reading_Integral_Yaw = 0,Reading_Integral_Yaw2 = 0;
+volatile int32_t MeanIntegralPitch, MeanIntegralPitch2;
+volatile int32_t MeanIntegralRoll, MeanIntegralRoll2;
+// attitude acceleration integrals
-volatile long MeanIntegralPitch, MeanIntegralRoll, MeanIntegralPitch2, MeanIntegralRoll2;
+volatile int32_t IntegralAccPitch = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
+volatile int32_t Reading_Integral_Top = 0;
-volatile long Reading_Integral_Top = 0;
+// compass course
-volatile int  CompassHeading = 0;
+volatile int16_t CompassHeading = 0;
-volatile int  CompassCourse = 0;
+volatile int16_t CompassCourse = 0;
+volatile int16_t CompassOffCourse = 0;
-volatile int  CompassOffCourse = 0;
-unsigned char MAX_GAS,MIN_GAS;
+// flags
-unsigned char EmergencyLanding = 0;
+uint8_t EmergencyLanding = 0;
+uint8_t HightControlActive = 0;
+uint8_t MotorsOn = 0;
-unsigned char HightControlActive = 0;
-long TurnOver180Pitch = 250000L, TurnOver180Roll = 250000L;
+int32_t TurnOver180Pitch = 250000L, TurnOver180Roll = 250000L;
+float GyroFactor;
-float GyroFactor;
+float IntegralFactor;
-float IntegralFactor;
+volatile int16_t  DiffPitch, DiffRoll;
+int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
-volatile int  DiffPitch,DiffRoll;
+// setpoints for motors
-int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
+volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left;
 volatile unsigned char Motor_Front, Motor_Rear, Motor_Right, Motor_Left, Count;
-unsigned char MotorValue[5];
-int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickGas = 0;
-char MotorsOn = 0;
-int ReadingHight = 0;
-int SetPointHight = 0;
-int AttitudeCorrectionRoll = 0, AttitudeCorrectionPitch = 0;
+// stick values derived by rc channels readings
-float Ki =  FAKTOR_I;
+int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickGas = 0;
-unsigned char Looping_Pitch = 0, Looping_Roll = 0;
-unsigned char Looping_Left = 0, Looping_Right = 0, Looping_Down = 0, Looping_Top = 0;
-unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
-unsigned char Parameter_MaxHoehe     = 251;     // Wert : 0-250
-unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
-unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
-unsigned char Parameter_CompassYawEffect = 64;  // Wert : 0-250
-unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
-unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
-unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
-unsigned char Parameter_I_Factor = 10;          // Wert : 1-20
-unsigned char Parameter_UserParam1 = 0;
-unsigned char Parameter_UserParam2 = 0;
-unsigned char Parameter_UserParam3 = 0;
-unsigned char Parameter_UserParam4 = 0;
-unsigned char Parameter_UserParam5 = 0;
-unsigned char Parameter_UserParam6 = 0;
-unsigned char Parameter_UserParam7 = 0;
-unsigned char Parameter_UserParam8 = 0;
-unsigned char Parameter_ServoPitchControl = 100;
-unsigned char Parameter_LoopGasLimit = 70;
-unsigned char Parameter_AchsKopplung1 = 0;
-unsigned char Parameter_AchsGegenKopplung1 = 0;
-unsigned char Parameter_DynamicStability = 100;
+// stick values derived by uart inputs
+int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHightValue = -20;
+int MaxStickPitch = 0, MaxStickRoll = 0;
+int16_t ReadingHight = 0;
+int16_t SetPointHight = 0;
+int16_t AttitudeCorrectionRoll = 0, AttitudeCorrectionPitch = 0;
+float Ki =  FACTOR_I;
+uint8_t Looping_Pitch = 0, Looping_Roll = 0;
+uint8_t Looping_Left = 0, Looping_Right = 0, Looping_Down = 0, Looping_Top = 0;
+fc_param_t FCParam = {48,251,16,58,64,150,150,2,10,0,0,0,0,0,0,0,0,100,70,0,0,100};
-signed int ExternStickPitch = 0, ExternStickRoll = 0,ExternStickYaw = 0, ExternHightValue = -20;
+/************************************************************************/
-int MaxStickPitch = 0, MaxStickRoll = 0;
+/*  Creates numbeeps beeps at the speaker                               */
+/************************************************************************/
-void Piep(unsigned char Anzahl)
+void Beep(uint8_t numbeeps)
 {
- while(Anzahl--)
+        while(numbeeps--)
+        {
+                if(MotorsOn) return; //auf keinen Fall im Flug!
- {
+                BeepTime = 100; // 0.1 second
-  if(MotorsOn) return; //auf keinen Fall im Flug!
+                Delay_ms(250); // blocks 250 ms as pause to next beep,
-  BeepTime = 100;
+                // this will block the flight control loop,
-  Delay_ms(250);
+                // therefore do not use this funktion if motors are running
+        }
+}
-}
-//############################################################################
+/************************************************************************/
-//  Nullwerte ermitteln
+/*  Neutral Readings                                                    */
-void SetNeutral(void)
+/************************************************************************/
-//############################################################################
+void SetNeutral(void)
+{
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
     AdNeutralPitch = 0;
         AdNeutralRoll = 0;
         AdNeutralYaw = 0;
-    Parameter_AchsKopplung1 = 0;
+    FCParam.AchsKopplung1 = 0;
-    Parameter_AchsGegenKopplung1 = 0;
+    FCParam.AchsGegenKopplung1 = 0;
     CalibMean();
     Delay_ms_Mess(100);
-        CalibMean();
-    if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL))  // Höhenregelung aktiviert?
+        CalibMean();
-     {
+    if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL))  // Hight Control activated?
-      if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset();
+    {
-     }
+                if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset();
+    }
-     AdNeutralPitch= AdValueGyrPitch;
+        AdNeutralPitch = AdValueGyrPitch;
-         AdNeutralRoll= AdValueGyrRoll;
+        AdNeutralRoll  = AdValueGyrRoll;
-         AdNeutralYaw= AdValueGyrYaw;
+        AdNeutralYaw   = AdValueGyrYaw;
-     StartNeutralRoll = AdNeutralRoll;
+        StartNeutralRoll  = AdNeutralRoll;
-     StartNeutralPitch = AdNeutralPitch;
+        StartNeutralPitch = AdNeutralPitch;
     if(GetParamByte(PID_ACC_PITCH) > 4)
+    {
-      NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY;
+                NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY;
-          NeutralAccX = abs(Mean_AccPitch) / ACC_AMPLIFY;
+                NeutralAccX = abs(Mean_AccPitch) / ACC_AMPLIFY;
-          NeutralAccZ = Current_AccZ;
+                NeutralAccZ = Current_AccZ;
     }
     else
-    { // why not GetParamWord()?
       NeutralAccX = (int)GetParamByte(PID_ACC_PITCH) * 256 + (int)GetParamByte(PID_ACC_PITCH+1);
-          NeutralAccY = (int)GetParamByte(PID_ACC_ROLL) * 256 + (int)GetParamByte(PID_ACC_ROLL+1);
+                NeutralAccX = (int16_t)GetParamWord(PID_ACC_PITCH);
-          NeutralAccZ = (int)GetParamByte(PID_ACC_Z) * 256 + (int)GetParamByte(PID_ACC_Z+1);
+            NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL);
             NeutralAccZ = (int16_t)GetParamWord(PID_ACC_Z);
+    }
         Reading_IntegralPitch = 0;
     Reading_IntegralPitch2 = 0;
     Reading_IntegralRoll = 0;
     Reading_IntegralRoll2 = 0;
-    Reading_Integral_Yaw = 0;
+    Reading_IntegralYaw = 0;
     ReadingPitch = 0;
     ReadingRoll = 0;
     ReadingYaw = 0;
     StartAirPressure = AirPressure;
-    HoeheD = 0;
+    HightD = 0;
     Reading_Integral_Top = 0;
     CompassCourse = CompassHeading;
     GPS_Neutral();
     BeepTime = 50;
-        TurnOver180Pitch = (long) ParamSet.AngleTurnOverPitch * 2500L;
+        TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
+        TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
-        TurnOver180Roll = (long) ParamSet.AngleTurnOverRoll * 2500L;
+    ExternHightValue = 0;
-    ExternHightValue = 0;
+}
-//############################################################################
-// Bearbeitet die Messwerte
-void Mean(void)
+/************************************************************************/
+/*  Averaging Measurement Readings                                      */
+/************************************************************************/
+void Mean(void)
+{
-//############################################################################
+    static int32_t tmpl,tmpl2;
-{
+ // Get offset corrected gyro readings
+    ReadingYaw   = AdNeutralYaw    - AdValueGyrYaw;
+    ReadingRoll  = AdValueGyrRoll  - AdNeutralRoll;
+    ReadingPitch = AdValueGyrPitch - AdNeutralPitch;
-    static int32_t tmpl,tmpl2;
-    ReadingYaw = (int16_t) AdNeutralYaw - AdValueGyrYaw;
-    ReadingRoll = (int16_t) AdValueGyrRoll - AdNeutralRoll;
-    ReadingPitch = (int16_t) AdValueGyrPitch - AdNeutralPitch;
         DebugOut.Analog[26] = ReadingPitch;
-//DebugOut.Analog[26] = ReadingPitch;
+        DebugOut.Analog[28] = ReadingRoll;
-DebugOut.Analog[28] = ReadingRoll;
+// Acceleration Sensor
+        Mean_AccPitch = ((int32_t)Mean_AccPitch * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccPitch))) / 2L;
-// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
+        Mean_AccRoll  = ((int32_t)Mean_AccRoll * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 2L;
-        Mean_AccPitch = ((long)Mean_AccPitch * 1 + ((ACC_AMPLIFY * (long)AdValueAccPitch))) / 2L;
+        Mean_AccTop   = ((int32_t)Mean_AccTop * 1 + ((int32_t)AdValueAccTop)) / 2L;
-        Mean_AccRoll = ((long)Mean_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdValueAccRoll))) / 2L;
+    IntegralAccPitch += ACC_AMPLIFY * AdValueAccPitch;
-        Mean_AccTop = ((long)Mean_AccTop * 1 + ((long)AdValueAccTop)) / 2L;
+    IntegralAccRoll  += ACC_AMPLIFY * AdValueAccRoll;
-    IntegralAccPitch += ACC_AMPLIFY * AdValueAccPitch;
+    IntegralAccZ     += Current_AccZ - NeutralAccZ;
-    IntegralAccRoll += ACC_AMPLIFY * AdValueAccRoll;
-    IntegralAccZ    += Current_AccZ - NeutralAccZ;
+// Yaw
-// Yaw  ++++++++++++++++++++++++++++++++++++++++++++++++
+    Reading_IntegralYaw +=  ReadingYaw;
-            Reading_Integral_Yaw +=  ReadingYaw;
+    Reading_IntegralYaw2 += ReadingYaw;
-            Reading_Integral_Yaw2 += ReadingYaw;
-// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
+// Coupling fraction
-      if(!Looping_Pitch && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
+        if(!Looping_Pitch && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
-         {
+        {
-            tmpl = Reading_IntegralPitch / 4096L;
+                tmpl = Reading_IntegralPitch / 4096L;
-            tmpl *= ReadingYaw;
+                tmpl *= ReadingYaw;
-            tmpl *= Parameter_AchsKopplung1;  //125
+                tmpl *= FCParam.AchsKopplung1;  //125
-            tmpl /= 2048L;
+                tmpl /= 2048L;
-            tmpl2 = Reading_IntegralRoll / 4096L;
+                tmpl2 = Reading_IntegralRoll / 4096L;
-            tmpl2 *= ReadingYaw;
+                tmpl2 *= ReadingYaw;
-            tmpl2 *= Parameter_AchsKopplung1;
+                tmpl2 *= FCParam.AchsKopplung1;
-            tmpl2 /= 2048L;
+                tmpl2 /= 2048L;
-         }
+        }
-      else  tmpl = tmpl2 = 0;
+        else  tmpl = tmpl2 = 0;
-// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
+// Roll
-            ReadingRoll += tmpl;
+        ReadingRoll += tmpl;
-            ReadingRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
+        ReadingRoll += (tmpl2 * FCParam.AchsGegenKopplung1) / 512L; //109
-            Reading_IntegralRoll2 += ReadingRoll;
+        Reading_IntegralRoll2 += ReadingRoll;
-            Reading_IntegralRoll +=  ReadingRoll - AttitudeCorrectionRoll;
+        Reading_IntegralRoll +=  ReadingRoll - AttitudeCorrectionRoll;
-            if(Reading_IntegralRoll > TurnOver180Roll)
+        if(Reading_IntegralRoll > TurnOver180Roll)
-            {
+        {
-             Reading_IntegralRoll  = -(TurnOver180Roll - 10000L);
+                Reading_IntegralRoll  = -(TurnOver180Roll - 10000L);
-             Reading_IntegralRoll2 = Reading_IntegralRoll;
+                Reading_IntegralRoll2 = Reading_IntegralRoll;
-            }
+        }
-            if(Reading_IntegralRoll <-TurnOver180Roll)
+        if(Reading_IntegralRoll < -TurnOver180Roll)
-            {
+        {
-             Reading_IntegralRoll =  (TurnOver180Roll - 10000L);
+                Reading_IntegralRoll =  (TurnOver180Roll - 10000L);
-             Reading_IntegralRoll2 = Reading_IntegralRoll;
+                Reading_IntegralRoll2 = Reading_IntegralRoll;
-            }
+        }
-            if(AdValueGyrRoll < 15)   ReadingRoll = -1000;
+        if(AdValueGyrRoll < 15)   ReadingRoll = -1000;
-            if(AdValueGyrRoll <  7)   ReadingRoll = -2000;
+        if(AdValueGyrRoll <  7)   ReadingRoll = -2000;
-            if(BoardRelease == 10)
+        if(BoardRelease == 10)
-                         {
+         {
-              if(AdValueGyrRoll > 1010) ReadingRoll = +1000;
+                if(AdValueGyrRoll > 1010) ReadingRoll = +1000;
-              if(AdValueGyrRoll > 1017) ReadingRoll = +2000;
+                if(AdValueGyrRoll > 1017) ReadingRoll = +2000;
-                         }
+         }
-                         else
+         else
-                         {
+         {
-              if(AdValueGyrRoll > 2020) ReadingRoll = +1000;
+                if(AdValueGyrRoll > 2020) ReadingRoll = +1000;
-              if(AdValueGyrRoll > 2034) ReadingRoll = +2000;
+                if(AdValueGyrRoll > 2034) ReadingRoll = +2000;
-                         }
+         }
-// Pitch  ++++++++++++++++++++++++++++++++++++++++++++++++
+// Pitch
-            ReadingPitch -= tmpl2;
+        ReadingPitch -= tmpl2;
-            ReadingPitch -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
+        ReadingPitch -= (tmpl*FCParam.AchsGegenKopplung1) / 512L;
-            Reading_IntegralPitch2 += ReadingPitch;
+        Reading_IntegralPitch2 += ReadingPitch;
-            Reading_IntegralPitch  += ReadingPitch - AttitudeCorrectionPitch;
+        Reading_IntegralPitch  += ReadingPitch - AttitudeCorrectionPitch;
-            if(Reading_IntegralPitch > TurnOver180Pitch)
+        if(Reading_IntegralPitch > TurnOver180Pitch)
-            {
+        {
-             Reading_IntegralPitch = -(TurnOver180Pitch - 10000L);
+         Reading_IntegralPitch = -(TurnOver180Pitch - 10000L);
-             Reading_IntegralPitch2 = Reading_IntegralPitch;
+         Reading_IntegralPitch2 = Reading_IntegralPitch;
-            }
+        }
-            if(Reading_IntegralPitch <-TurnOver180Pitch)
+        if(Reading_IntegralPitch < -TurnOver180Pitch)
-            {
+        {
-             Reading_IntegralPitch =  (TurnOver180Pitch - 10000L);
+         Reading_IntegralPitch =  (TurnOver180Pitch - 10000L);
-             Reading_IntegralPitch2 = Reading_IntegralPitch;
+         Reading_IntegralPitch2 = Reading_IntegralPitch;
-            }
+        }
-            if(AdValueGyrPitch < 15)   ReadingPitch = -1000;
+        if(AdValueGyrPitch < 15)   ReadingPitch = -1000;
-            if(AdValueGyrPitch <  7)   ReadingPitch = -2000;
+        if(AdValueGyrPitch <  7)   ReadingPitch = -2000;
-            if(BoardRelease == 10)
+        if(BoardRelease == 10)
-                         {
+         {
-              if(AdValueGyrPitch > 1010) ReadingPitch = +1000;
+                if(AdValueGyrPitch > 1010) ReadingPitch = +1000;
-              if(AdValueGyrPitch > 1017) ReadingPitch = +2000;
+                if(AdValueGyrPitch > 1017) ReadingPitch = +2000;
+         }
-                         else
-                         {
+         else
               if(AdValueGyrPitch > 2020) ReadingPitch = +1000;
-              if(AdValueGyrPitch > 2034) ReadingPitch = +2000;
+                if(AdValueGyrPitch > 2020) ReadingPitch = +1000;
                 if(AdValueGyrPitch > 2034) ReadingPitch = +2000;
 //++++++++++++++++++++++++++++++++++++++++++++++++
-// ADC einschalten
-    ADC_Enable();
+// start ADC
-//++++++++++++++++++++++++++++++++++++++++++++++++
+    ADC_Enable();
-    Integral_Yaw  = Reading_Integral_Yaw;
+    IntegralYaw  = Reading_IntegralYaw;
     IntegralPitch = Reading_IntegralPitch;
     IntegralRoll = Reading_IntegralRoll;
+    IntegralPitch2 = Reading_IntegralPitch2;
-    IntegralPitch2 = Reading_IntegralPitch2;
+    IntegralRoll2 = Reading_IntegralRoll2;
     IntegralRoll2 = Reading_IntegralRoll2;
         if(ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER && !Looping_Pitch && !Looping_Roll)
   if(ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER && !Looping_Pitch && !Looping_Roll)
+        if(ReadingPitch > 200)       ReadingPitch += 4 * (ReadingPitch - 200);
         else if(ReadingPitch < -200) ReadingPitch += 4 * (ReadingPitch + 200);
-    if(ReadingPitch > 200)       ReadingPitch += 4 * (ReadingPitch - 200);
+        if(ReadingRoll > 200)        ReadingRoll  += 4 * (ReadingRoll - 200);
-    else if(ReadingPitch < -200) ReadingPitch += 4 * (ReadingPitch + 200);
+        else if(ReadingRoll < -200)  ReadingRoll  += 4 * (ReadingRoll + 200);
     if(ReadingRoll > 200)       ReadingRoll += 4 * (ReadingRoll - 200);
-    else if(ReadingRoll < -200) ReadingRoll += 4 * (ReadingRoll + 200);
+        //update poti values by rc-signals (why not +127?)
-  }
+    if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
-    if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
+    if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
+    if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
-    if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
+    if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
-    if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
-    if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
+        //limit poti values
     if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
     if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
+    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
-    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
+    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
     if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
+/************************************************************************/
+/*  Averaging Measurement Readings  for Calibration                     */
-//############################################################################
+/************************************************************************/
-// Messwerte beim Ermitteln der Nullage
+void CalibMean(void)
-void CalibMean(void)
+{
-//############################################################################
+    // stop ADC to avoid changing values during calculation
+        ADC_Disable();
-    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
+        ReadingPitch = AdValueGyrPitch;
-        ADC_Disable();
+        ReadingRoll = AdValueGyrRoll;
-        ReadingPitch = AdValueGyrPitch;
+        ReadingYaw = AdValueGyrYaw;
-        ReadingRoll = AdValueGyrRoll;
+        Mean_AccPitch = ACC_AMPLIFY * (int32_t)AdValueAccPitch;
-        ReadingYaw = AdValueGyrYaw;
+        Mean_AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
-        Mean_AccPitch = ACC_AMPLIFY * (long)AdValueAccPitch;
+        Mean_AccTop = (int32_t)AdValueAccTop;
-        Mean_AccRoll = ACC_AMPLIFY * (long)AdValueAccRoll;
+   // start ADC
-        Mean_AccTop = (long)AdValueAccTop;
+    ADC_Enable();
-   // ADC einschalten
+    //update poti values by rc-signals (why not +127?)
-    ADC_Enable();
+    if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
-    if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
+    if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
-    if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
+    if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
+    if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
-    if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
+        //limit poti values
-    if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
     if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
     if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
     if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
     if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
-        TurnOver180Pitch = (long) ParamSet.AngleTurnOverPitch * 2500L;
+        TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
-        TurnOver180Roll = (long) ParamSet.AngleTurnOverRoll * 2500L;
+        TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
+}
-//############################################################################
+/************************************************************************/
-// Senden der Motorwerte per I2C-Bus
+/*  Transmit Motor Data via I2C                                         */
-void SendMotorData(void)
+/************************************************************************/
-//############################################################################
+void SendMotorData(void)
+{
     if(MOTOR_OFF || !MotorsOn)
+    {
     i2c_start();
+}
+/************************************************************************/
-//############################################################################
-// Trägt ggf. das Poti als Parameter ein
+/*  Maps the parameter to poti values                                   */
-void ParameterZuordnung(void)
+/************************************************************************/
-//############################################################################
+void ParameterMapping(void)
 {
  #define CHK_POTI(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;}
- CHK_POTI(Parameter_MaxHoehe,ParamSet.MaxHight,0,255);
+ CHK_POTI(FCParam.MaxHight,ParamSet.MaxHight,0,255);
- CHK_POTI(Parameter_Luftdruck_D,ParamSet.AirPressure_D,0,100);
+ CHK_POTI(FCParam.AirPressure_D,ParamSet.AirPressure_D,0,100);
- CHK_POTI(Parameter_Hoehe_P,ParamSet.Hight_P,0,100);
+ CHK_POTI(FCParam.Hight_P,ParamSet.Hight_P,0,100);
- CHK_POTI(Parameter_Hoehe_ACC_Wirkung,ParamSet.Hight_ACC_Effect,0,255);
+ CHK_POTI(FCParam.Hight_ACC_Effect,ParamSet.Hight_ACC_Effect,0,255);
- CHK_POTI(Parameter_CompassYawEffect,ParamSet.CompassYawEffect,0,255);
+ CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255);
- CHK_POTI(Parameter_Gyro_P,ParamSet.Gyro_P,10,255);
+ CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
- CHK_POTI(Parameter_Gyro_I,ParamSet.Gyro_I,0,255);
+ CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255);
- CHK_POTI(Parameter_I_Factor,ParamSet.I_Factor,0,255);
+ CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255);
- CHK_POTI(Parameter_UserParam1,ParamSet.UserParam1,0,255);
+ CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1,0,255);
- CHK_POTI(Parameter_UserParam2,ParamSet.UserParam2,0,255);
+ CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2,0,255);
- CHK_POTI(Parameter_UserParam3,ParamSet.UserParam3,0,255);
+ CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3,0,255);
- CHK_POTI(Parameter_UserParam4,ParamSet.UserParam4,0,255);
+ CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4,0,255);
- CHK_POTI(Parameter_UserParam5,ParamSet.UserParam5,0,255);
+ CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255);
- CHK_POTI(Parameter_UserParam6,ParamSet.UserParam6,0,255);
+ CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255);
- CHK_POTI(Parameter_UserParam7,ParamSet.UserParam7,0,255);
+ CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255);
- CHK_POTI(Parameter_UserParam8,ParamSet.UserParam8,0,255);
+ CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255);
- CHK_POTI(Parameter_ServoPitchControl,ParamSet.ServoPitchControl,0,255);
+ CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255);
- CHK_POTI(Parameter_LoopGasLimit,ParamSet.LoopGasLimit,0,255);
+ CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit,0,255);
- CHK_POTI(Parameter_AchsKopplung1,    ParamSet.AchsKopplung1,0,255);
+ CHK_POTI(FCParam.AchsKopplung1,    ParamSet.AchsKopplung1,0,255);
- CHK_POTI(Parameter_AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255);
- CHK_POTI(Parameter_DynamicStability,ParamSet.DynamicStability,0,255);
  CHK_POTI(FCParam.AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255);
- Ki = (float) Parameter_I_Factor * 0.0001;
+ CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255);
  MAX_GAS = ParamSet.Gas_Max;
+                }
             if((modell_fliegt < 200) || (GasMixingFraction < 40))
+                {
                 SumPitch = 0;
                 SumRoll = 0;
-                Reading_Integral_Yaw = 0;
+                Reading_IntegralYaw = 0;
-                Reading_Integral_Yaw2 = 0;
+                Reading_IntegralYaw2 = 0;
+                }
             if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // auf Nullwerte kalibrieren
+                          {
                              if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset();
+                          }
                             ParamSet_ReadFromEEProm(GetActiveParamSet());
                         SetNeutral();
-                        Piep(GetActiveParamSet());
+                        Beep(GetActiveParamSet());
+                        }
+                    }
                  else
                 if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)  // ACC Neutralwerte speichern
+                    {
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                         GRN_OFF;
-                        SetParamByte(PID_ACC_PITCH,0xFF); // Werte löschen
+                        SetParamWord(PID_ACC_PITCH,0xFFFF); // Werte löschen
                         MotorsOn = 0;
                         delay_neutral = 0;
                         modell_fliegt = 0;
                         SetNeutral();
-                        SetParamByte(PID_ACC_PITCH, NeutralAccX / 256); // ACC-NeutralWerte speichern
-                        SetParamByte(PID_ACC_PITCH+1,NeutralAccX % 256); // ACC-NeutralWerte speichern
-                        SetParamByte(PID_ACC_ROLL,NeutralAccY / 256);
+                        // ACC-NeutralWerte speichern
-                        SetParamByte(PID_ACC_ROLL+1,NeutralAccY % 256);
+                        SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX);
-                        SetParamByte(PID_ACC_Z,(int)NeutralAccZ / 256);
+                        SetParamWord(PID_ACC_ROLL,  (uint16_t)NeutralAccY);
-                        SetParamByte(PID_ACC_Z+1,(int)NeutralAccZ % 256);
+                        SetParamWord(PID_ACC_Z,     (uint16_t)NeutralAccZ);
-                        Piep(GetActiveParamSet());
+                        Beep(GetActiveParamSet());
+                        }
+                    }
                  else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+                        {
                         delay_einschalten = 200;
                         modell_fliegt = 1;
                         MotorsOn = 1;
                         SetPointYaw = 0;
-                        Reading_Integral_Yaw = 0;
+                        Reading_IntegralYaw = 0;
-                        Reading_Integral_Yaw2 = 0;
+                        Reading_IntegralYaw2 = 0;
                         Reading_IntegralPitch = 0;
                         Reading_IntegralRoll = 0;
                         Reading_IntegralPitch2 = IntegralPitch;
                         Reading_IntegralRoll2 = IntegralRoll;
                         SumPitch = 0;
 // neue Werte von der Funke
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!NewPpmData-- || EmergencyLanding)
+  {
     int tmp_int;
-    ParameterZuordnung();
+    ParameterMapping();
     StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4;
     StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D;
     StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
     StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
      MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]); else MaxStickPitch--;
    if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll)
      MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); else MaxStickRoll--;
    if(EmergencyLanding)  {MaxStickPitch = 0; MaxStickRoll = 0;}
-    GyroFactor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
+    GyroFactor     = ((float)FCParam.Gyro_P + 10.0) / 256.0;
-    IntegralFactor = ((float) Parameter_Gyro_I) / 44000;
+    IntegralFactor = ((float) FCParam.Gyro_I) / 44000;
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Digitale Steuerung per DubWise
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-#define KEY_VALUE (Parameter_UserParam1 * 4)  //(Poti3 * 8)
+#define KEY_VALUE (FCParam.UserParam1 * 4)  //(Poti3 * 8)
 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;
     StickRoll += ExternStickRoll / 8;
     StickYaw += ExternStickYaw;
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Analoge Steuerung per Seriell
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-   if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)
+   if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128)
+    {
          StickPitch += (int) ExternControl.Pitch * (int) ParamSet.Stick_P;
          StickRoll += (int) ExternControl.Roll * (int) ParamSet.Stick_P;
          StickYaw += ExternControl.Yaw;
      ExternHightValue =  (int) ExternControl.Hight * (int)ParamSet.Hight_Gain;
       if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) StoreNewCompassCourse = 1;
+     }
     tmp_int  = (long) ParamSet.Yaw_P * ((long)StickYaw * abs(StickYaw)) / 512L; // expo  y = ax + bx²
     tmp_int += (ParamSet.Yaw_P * StickYaw) / 4;
     SetPointYaw = tmp_int;
-    Reading_Integral_Yaw -= tmp_int;
+    Reading_IntegralYaw -= tmp_int;
-    if(Reading_Integral_Yaw > 50000) Reading_Integral_Yaw = 50000;  // begrenzen
+    if(Reading_IntegralYaw > 50000) Reading_IntegralYaw = 50000;  // begrenzen
-    if(Reading_Integral_Yaw <-50000) Reading_Integral_Yaw =-50000;
+    if(Reading_IntegralYaw <-50000) Reading_IntegralYaw =-50000;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(w < 35 && StoreNewCompassCourse)
+         {
           CompassCourse = CompassHeading;
           StoreNewCompassCourse = 0;
+         }
-        w = (w * Parameter_CompassYawEffect) / 64;           // auf die Wirkung normieren
+        w = (w * FCParam.CompassYawEffect) / 64;           // auf die Wirkung normieren
-        w = Parameter_CompassYawEffect - w;                  // Wirkung ggf drosseln
+        w = FCParam.CompassYawEffect - w;                  // Wirkung ggf drosseln
         if(w > 0)
+        {
-                        Reading_Integral_Yaw -= (CompassOffCourse * w) / 32;  // nach Kompass ausrichten
+                        Reading_IntegralYaw -= (CompassOffCourse * w) / 32;  // nach Kompass ausrichten
+           }
+     }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DebugOut.Analog[24] = motor_rx[7];
     DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
 */
 //    DebugOut.Analog[9] = ReadingPitch;
 //    DebugOut.Analog[9] = SetPointHight;
-//    DebugOut.Analog[10] = Reading_Integral_Yaw / 128;
+//    DebugOut.Analog[10] = Reading_IntegralYaw / 128;
 //    DebugOut.Analog[11] = CompassCourse;
-//    DebugOut.Analog[10] = Parameter_Gyro_I;
+//    DebugOut.Analog[10] = FCParam.Gyro_I;
 //    DebugOut.Analog[10] = ParamSet.Gyro_I;
 //    DebugOut.Analog[9] = CompassOffCourse;
 //    DebugOut.Analog[10] = GasMixingFraction;
-//    DebugOut.Analog[3] = HoeheD * 32;
+//    DebugOut.Analog[3] = HightD * 32;
 //    DebugOut.Analog[4] = hoehenregler;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(Looping_Pitch) ReadingPitch = ReadingPitch * GyroFactor;
     else             ReadingPitch = IntegralPitch * IntegralFactor + ReadingPitch * GyroFactor;
     if(Looping_Roll) ReadingRoll = ReadingRoll * GyroFactor;
     else             ReadingRoll = IntegralRoll * IntegralFactor + ReadingRoll * GyroFactor;
-    ReadingYaw = ReadingYaw * (2 * GyroFactor) + Integral_Yaw * IntegralFactor / 2;
+    ReadingYaw = ReadingYaw * (2 * GyroFactor) + IntegralYaw * IntegralFactor / 2;
 DebugOut.Analog[25] = IntegralRoll * IntegralFactor;
  if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL))  // Höhenregelung
+  {
     int tmp_int;
     if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)  // Regler wird über Schalter gesteuert
+    {
-     if(Parameter_MaxHoehe < 50)
+     if(FCParam.MaxHight < 50)
+      {
-       SetPointHight = ReadingHight - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
+       SetPointHight = ReadingHight - 20;  // FCParam.MaxHight ist der PPM-Wert des Schalters
        HightControlActive = 0;
+      }
       else
         HightControlActive = 1;
+    }
     else
+    {
-     SetPointHight = ((int) ExternHightValue + (int) Parameter_MaxHoehe) * (int)ParamSet.Hight_Gain - 20;
+     SetPointHight = ((int) ExternHightValue + (int) FCParam.MaxHight) * (int)ParamSet.Hight_Gain - 20;
      HightControlActive = 1;
+    }
     if(EmergencyLanding) SetPointHight = 0;
     h = ReadingHight;
     if((h > SetPointHight) && HightControlActive)      // zu hoch --> drosseln
-     {      h = ((h - SetPointHight) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
+     {      h = ((h - SetPointHight) * (int) FCParam.Hight_P) / 16; // Differenz bestimmen --> P-Anteil
       h = GasMixingFraction - h;         // vom Gas abziehen
-      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
+      h -= (HightD * FCParam.AirPressure_D)/8;    // D-Anteil
-      tmp_int = ((Reading_Integral_Top / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
+      tmp_int = ((Reading_Integral_Top / 512) * (signed long) FCParam.Hight_ACC_Effect) / 32;
       if(tmp_int > 50) tmp_int = 50;
       else if(tmp_int < -50) tmp_int = -50;
       h -= tmp_int;
       hoehenregler = (hoehenregler*15 + h) / 16;
+       }
       if(hoehenregler > GasMixingFraction) hoehenregler = GasMixingFraction; // nicht mehr als Gas
       GasMixingFraction = hoehenregler;
+     }
+  }
-  if(GasMixingFraction > MAX_GAS - 20) GasMixingFraction = MAX_GAS - 20;
+  if(GasMixingFraction > ParamSet.Gas_Max - 20) GasMixingFraction = ParamSet.Gas_Max - 20;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   DebugOut.Analog[7] = GasMixingFraction;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     YawMixingFraction = ReadingYaw - SetPointYaw;     // Regler für Gier
 // YawMixingFraction = 0;
     if(YawMixingFraction > (GasMixingFraction / 2)) YawMixingFraction = GasMixingFraction / 2;
     if(YawMixingFraction < -(GasMixingFraction / 2)) YawMixingFraction = -(GasMixingFraction / 2);
-    if(YawMixingFraction > ((MAX_GAS - GasMixingFraction))) YawMixingFraction = ((MAX_GAS - GasMixingFraction));
+    if(YawMixingFraction > ((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = ((ParamSet.Gas_Max - GasMixingFraction));
-    if(YawMixingFraction < -((MAX_GAS - GasMixingFraction))) YawMixingFraction = -((MAX_GAS - GasMixingFraction));
+    if(YawMixingFraction < -((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = -((ParamSet.Gas_Max - GasMixingFraction));
     if(GasMixingFraction < 20) YawMixingFraction = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     else  SumPitch += DiffPitch; // I-Anteil bei HH
     if(SumPitch >  16000) SumPitch =  16000;
     if(SumPitch < -16000) SumPitch = -16000;
     pd_ergebnis = DiffPitch + Ki * SumPitch; // PI-Regler für Pitch
     // Motor Vorn
-    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64;
+    tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
     if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
     MotorValue = GasMixingFraction + pd_ergebnis + YawMixingFraction;     // Mischer
         if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > MAX_GAS)       MotorValue = MAX_GAS;
+        else if(MotorValue > ParamSet.Gas_Max)              MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < MIN_GAS)            MotorValue = MIN_GAS;
+        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
         Motor_Front = MotorValue;
     // Motor Heck
         MotorValue = GasMixingFraction - pd_ergebnis + YawMixingFraction;
         if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > MAX_GAS)       MotorValue = MAX_GAS;
+        else if(MotorValue > ParamSet.Gas_Max)      MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < MIN_GAS)            MotorValue = MIN_GAS;
+        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
         Motor_Rear = MotorValue;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = ReadingRoll - (StickRoll  - GPS_Roll);       // Differenz bestimmen
     if(IntegralFactor) SumRoll += IntegralRoll * IntegralFactor - (StickRoll  - GPS_Roll);// I-Anteil bei Winkelregelung
     else                 SumRoll += DiffRoll;  // I-Anteil bei HH
     if(SumRoll >  16000) SumRoll =  16000;
     if(SumRoll < -16000) SumRoll = -16000;
     pd_ergebnis = DiffRoll + Ki * SumRoll;      // PI-Regler für Roll
-    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64;
+    tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
     if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
     // Motor Links
     MotorValue = GasMixingFraction + pd_ergebnis - YawMixingFraction;
 #define GRENZE Poti1
         if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > MAX_GAS)           MotorValue = MAX_GAS;
+        else if(MotorValue > ParamSet.Gas_Max)          MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < MIN_GAS)            MotorValue = MIN_GAS;
+        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
     Motor_Left = MotorValue;
     // Motor Rechts
         MotorValue = GasMixingFraction - pd_ergebnis - YawMixingFraction;
         if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > MAX_GAS)           MotorValue = MAX_GAS;
+        else if(MotorValue > ParamSet.Gas_Max)          MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < MIN_GAS)            MotorValue = MIN_GAS;
+        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/fc.c @ 703 - Rev 701 → 703