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

 // stick values derived by rc channels readings
 int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0;
 int16_t MaxStickPitch = 0, MaxStickRoll = 0, MaxStickYaw = 0;
 // stick values derived by uart inputs
-int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHightValue = -20;
+int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20;
     FCParam.Yaw_PosFeedback = 0;
     FCParam.Yaw_NegFeedback = 0;
     CalibMean();
     Delay_ms_Mess(100);
         CalibMean();
-    if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL))  // Hight Control activated?
+    if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL))  // Height Control activated?
+    {
                 if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset();
+    }
         AdNeutralPitch = AdValueGyrPitch;
         AdNeutralRoll  = AdValueGyrRoll;
     Reading_IntegralGyroYaw = 0;
     Reading_GyroPitch = 0;
     Reading_GyroRoll = 0;
     Reading_GyroYaw = 0;
     StartAirPressure = AirPressure;
-    HightD = 0;
+    HeightD = 0;
     Reading_Integral_Top = 0;
     CompassCourse = CompassHeading;
     BeepTime = 50;
         TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
         TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
-    ExternHightValue = 0;
+    ExternHeightValue = 0;
     GPS_Neutral();
+}
 /************************************************************************/
 /************************************************************************/
 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(FCParam.MaxHight,ParamSet.MaxHight,0,255);
+        CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255);
-        CHK_POTI(FCParam.Hight_D,ParamSet.Hight_D,0,100);
+        CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
-        CHK_POTI(FCParam.Hight_P,ParamSet.Hight_P,0,100);
+        CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
-        CHK_POTI(FCParam.Hight_ACC_Effect,ParamSet.Hight_ACC_Effect,0,255);
+        CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
         CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255);
         CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
         CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255);
         CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255);
      static int32_t IntegralErrorPitch = 0;
      static int32_t IntegralErrorRoll = 0;
          static uint16_t RcLostTimer;
          static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
          static uint16_t Modell_Is_Flying = 0;
-         static uint8_t HightControlActive = 0;
+         static uint8_t HeightControlActive = 0;
-     static int16_t HightControlThrust = 0;
+     static int16_t HeightControlThrust = 0;
      static int8_t TimerDebugOut = 0;
      static int8_t StoreNewCompassCourse = 0;
      static int32_t CorrectionPitch, CorrectionRoll;
                         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)  ExternHightValue++;
+                        if(DubWiseKeys[0] & 2)  ExternHeightValue++;
-                        if(DubWiseKeys[0] & 16) ExternHightValue--;
+                        if(DubWiseKeys[0] & 16) ExternHeightValue--;
                         StickPitch += ExternStickPitch / 8;
                     if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128)
+                        {
                                  StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P;
                                  StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P;
                                  StickYaw += ExternControl.Yaw;
-                                 ExternHightValue =  (int16_t) ExternControl.Hight * (int16_t)ParamSet.Hight_Gain;
+                                 ExternHeightValue =  (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain;
                                  if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust;
+                        }
             // disable I part of gyro control feedback
                         if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor =  0;
                         // avoid negative scaling factors
                         DebugOut.Analog[0]  = IntegralPitch / ParamSet.GyroAccFaktor;
                         DebugOut.Analog[1]  = IntegralRoll / ParamSet.GyroAccFaktor;
                         DebugOut.Analog[2]  = Mean_AccPitch;
                         DebugOut.Analog[3]  = Mean_AccRoll;
                         DebugOut.Analog[4]  = Reading_GyroYaw;
-                        DebugOut.Analog[5]  = ReadingHight;
+                        DebugOut.Analog[5]  = ReadingHeight;
                         DebugOut.Analog[6]  = (Reading_Integral_Top / 512);
                         DebugOut.Analog[8]  = CompassHeading;
                         DebugOut.Analog[9]  = UBat;
                         DebugOut.Analog[10] = SenderOkay;
                         DebugOut.Analog[16] = Mean_AccTop;
                         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]  = SetPointHight;
+                        DebugOut.Analog[9]  = SetPointHeight;
                         DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128;
                         DebugOut.Analog[11] = CompassCourse;
                         DebugOut.Analog[10] = FCParam.Gyro_I;
                         DebugOut.Analog[10] = ParamSet.Gyro_I;
                         DebugOut.Analog[9]  = CompassOffCourse;
                         DebugOut.Analog[10] = ThrustMixFraction;
-                        DebugOut.Analog[3]  = HightD * 32;
+                        DebugOut.Analog[3]  = HeightD * 32;
-                        DebugOut.Analog[4]  = HightControlThrust;
+                        DebugOut.Analog[4]  = HeightControlThrust;
                         */
+                }
                 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;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Hight Control
+// Height Control
 // The higth control algorithm reduces the thrust but does not increase the thrust.
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 // If hight control is activated and no emergency landing is active
                 if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
+                {
                         int tmp_int;
                         // if hight control is activated by an rc channel
                         if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
                         {       // check if parameter is less than activation threshold
-                                if(FCParam.MaxHight < 50)
+                                if(FCParam.MaxHeight < 50)
                                 {
-                                        SetPointHight = ReadingHight - 20;  // update SetPoint with current reading
+                                        SetPointHeight = ReadingHeight - 20;  // update SetPoint with current reading
-                                        HightControlActive = 0; // disable hight control
+                                        HeightControlActive = 0; // disable hight control
                                 }
-                                else HightControlActive = 1; // enable hight control
+                                else HeightControlActive = 1; // enable hight control
+                        }
                         else // no switchable hight control
                         {
-                                SetPointHight = ((int16_t) ExternHightValue + (int16_t) FCParam.MaxHight) * (int16_t)ParamSet.Hight_Gain - 20;
+                                SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
-                                HightControlActive = 1;
+                                HeightControlActive = 1;
+                        }
                         // get current hight
-                        h = ReadingHight;
+                        h = ReadingHeight;
                         // if current hight is above the setpoint reduce thrust
-                        if((h > SetPointHight) && HightControlActive)
+                        if((h > SetPointHeight) && HeightControlActive)
+                        {
                                 // hight difference -> P control part
-                                h = ((h - SetPointHight) * (int16_t) FCParam.Hight_P) / 16;
+                                h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / 16;
                                 h = ThrustMixFraction - h; // reduce gas
                                 // higth gradient --> D control part
-                                h -= (HightD * FCParam.Hight_D) / 8;  // D control part
+                                h -= (HeightD * FCParam.Height_D) / 8;  // D control part
                                 // acceleration sensor effect
-                                tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Hight_ACC_Effect) / 32;
+                                tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Height_ACC_Effect) / 32;
                                 if(tmp_int > 50) tmp_int = 50;
                                 if(tmp_int < -50) tmp_int = -50;
                                 h -= tmp_int;
                                 // update hight control thrust
-                                HightControlThrust = (HightControlThrust*15 + h) / 16;
+                                HeightControlThrust = (HeightControlThrust*15 + h) / 16;
                                 // limit thrust reduction
-                                if(HightControlThrust < ParamSet.Hight_MinThrust)
+                                if(HeightControlThrust < ParamSet.Height_MinThrust)
                                 {
-                                        if(ThrustMixFraction >= ParamSet.Hight_MinThrust) HightControlThrust = ParamSet.Hight_MinThrust;
+                                        if(ThrustMixFraction >= ParamSet.Height_MinThrust) HeightControlThrust = ParamSet.Height_MinThrust;
                                         // allows landing also if thrust stick is reduced below min thrust on hight control
-                                        if(ThrustMixFraction < ParamSet.Hight_MinThrust) HightControlThrust = ThrustMixFraction;
+                                        if(ThrustMixFraction < ParamSet.Height_MinThrust) HeightControlThrust = ThrustMixFraction;
+                                }
                                 // limit thrust to stick setting
-                                if(HightControlThrust > ThrustMixFraction) HightControlThrust = ThrustMixFraction;
+                                if(HeightControlThrust > ThrustMixFraction) HeightControlThrust = ThrustMixFraction;
-                                ThrustMixFraction = HightControlThrust;
+                                ThrustMixFraction = HeightControlThrust;
+                        }
+                }
                 // limit thrust to parameter setting
                 if(ThrustMixFraction > ParamSet.Trust_Max - 20) ThrustMixFraction = ParamSet.Trust_Max - 20;

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/fc.c - Rev 754 → 761