WebSVN - FlightCtrl - Diff - Rev 706 and 707 - /branches/V0.68d CRK HexaLotte/fc.c

 // 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, StickGas = 0;
+int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0;
 // stick values derived by uart inputs
  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.LoopGasLimit,ParamSet.LoopGasLimit,0,255);
+ CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,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);
 /*  MotorControl                                                        */
 /************************************************************************/
 void MotorRegler(void)
+{
          int16_t MotorValue, pd_ergebnis, h, tmp_int;
-         int16_t YawMixingFraction, GasMixingFraction;
+         int16_t YawMixingFraction, ThrustMixingFraction;
      static int32_t SumPitch = 0, SumRoll = 0;
      static int32_t SetPointYaw = 0, tmp_long, tmp_long2;
      static int32_t IntegralErrorPitch = 0;
      static int32_t IntegralErrorRoll = 0;
          static uint16_t RcLostTimer;
         Mean();
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// determine GAS value
+// determine thrust value
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        GasMixingFraction = StickGas;
+        ThrustMixingFraction = StickThrust;
-    if(GasMixingFraction < 0) GasMixingFraction = 0;
+    if(ThrustMixingFraction < 0) ThrustMixingFraction = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // RC-signal is bad
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                   EmergencyLanding = 0; // emergency landing is over
+                }
                 ROT_ON; // set red led
                 if(Modell_Is_Flying > 2000)  // wahrscheinlich in der Luft --> langsam absenken
+                {
-                        GasMixingFraction = ParamSet.EmergencyGas; // set emergency gas
+                        ThrustMixingFraction = ParamSet.EmergencyThrust; // set emergency thrust
                         EmergencyLanding = 1; // enable emergency landing
                         // set neutral rc inputs
                         PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0;
                         PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
                         PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(SenderOkay > 140)
+        {
                 EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay
                 // reset emergency timer
-                RcLostTimer = ParamSet.EmergencyGasDuration * 50;
+                RcLostTimer = ParamSet.EmergencyThrustDuration * 50;
-                if(GasMixingFraction > 40)
+                if(ThrustMixingFraction > 40)
+                {
                         if(Modell_Is_Flying < 0xFFFF) Modell_Is_Flying++;
+                }
-                if((Modell_Is_Flying < 200) || (GasMixingFraction < 40))
+                if((Modell_Is_Flying < 200) || (ThrustMixingFraction < 40))
+                {
                         SumPitch = 0;
                         SumRoll = 0;
                         Reading_IntegralYaw = 0;
                         Reading_IntegralYaw2 = 0;
+                }
                 // if motors are off and the thrust stick is in the upper position
-                if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0)
+                if((PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 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
             else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // thrust stick is down
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85)
+                if(PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < -85)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // and yaw stick is rightmost --> start motors
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
                         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;
-                        // direct mapping of yaw and gas
+                        // direct mapping of yaw and thrust
                         StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
-                        StickGas  = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers
+                        StickThrust  = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] + 120;// shift to positive numbers
                         // update max stick positions for pitch and roll
                         if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch)
+                        {
                                  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;
-                                 if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
+                                 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
                         if(Gyro_P_Factor < 0) Gyro_P_Factor = 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;
-                } // End of new RC values
+                } // End of new RC-Values or Emergency Landing
                 if(Looping_Roll) BeepTime = 100;
                 if(Looping_Roll || Looping_Pitch)
+                {
-                if(GasMixingFraction > ParamSet.LoopGasLimit) GasMixingFraction = ParamSet.LoopGasLimit;
+                if(ThrustMixingFraction > ParamSet.LoopThrustLimit) ThrustMixingFraction = ParamSet.LoopThrustLimit;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // in case of emergency landing
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 // set all inputs to save values
                 if(EmergencyLanding)
+                        {
                         tmp_long  /= 3;
                         tmp_long2 /= 3;
+                        }
-                        #define AUSGLEICH 32
+                        #define BALANCE 32
-                        if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
+                        if(tmp_long >  BALANCE)  tmp_long  = BALANCE;
-                        if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
+                        if(tmp_long < -BALANCE)  tmp_long  =-BALANCE;
-                        if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
+                        if(tmp_long2 > BALANCE)  tmp_long2 = BALANCE;
-                        if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
+                        if(tmp_long2 <-BALANCE)  tmp_long2 =-BALANCE;
                         Reading_IntegralPitch -= tmp_long;
                         Reading_IntegralRoll -= tmp_long2;
 //    DebugOut.Analog[10] = Reading_IntegralYaw / 128;
 //    DebugOut.Analog[11] = CompassCourse;
 //    DebugOut.Analog[10] = FCParam.Gyro_I;
 //    DebugOut.Analog[10] = ParamSet.Gyro_I;
 //    DebugOut.Analog[9] = CompassOffCourse;
-//    DebugOut.Analog[10] = GasMixingFraction;
+//    DebugOut.Analog[10] = ThrustMixingFraction;
 //    DebugOut.Analog[3] = HightD * 32;
 //    DebugOut.Analog[4] = hoehenregler;
+  }
     if(EmergencyLanding) SetPointHight = 0;
     h = ReadingHight;
     if((h > SetPointHight) && HightControlActive)      // zu hoch --> drosseln
      {      h = ((h - SetPointHight) * (int) FCParam.Hight_P) / 16; // Differenz bestimmen --> P-Anteil
-      h = GasMixingFraction - h;         // vom Gas abziehen
+      h = ThrustMixingFraction - h;         // vom Gas abziehen
       h -= (HightD * FCParam.AirPressure_D)/8;    // D-Anteil
       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 < ParamSet.Hight_MinGas) // nicht unter MIN
+      if(hoehenregler < ParamSet.Hight_MinThrust) // nicht unter MIN
        {
-         if(GasMixingFraction >= ParamSet.Hight_MinGas) hoehenregler = ParamSet.Hight_MinGas;
+         if(ThrustMixingFraction >= ParamSet.Hight_MinThrust) hoehenregler = ParamSet.Hight_MinThrust;
-         if(GasMixingFraction < ParamSet.Hight_MinGas) hoehenregler = GasMixingFraction;
+         if(ThrustMixingFraction < ParamSet.Hight_MinThrust) hoehenregler = ThrustMixingFraction;
        }
-      if(hoehenregler > GasMixingFraction) hoehenregler = GasMixingFraction; // nicht mehr als Gas
+      if(hoehenregler > ThrustMixingFraction) hoehenregler = ThrustMixingFraction; // nicht mehr als Gas
-      GasMixingFraction = hoehenregler;
+      ThrustMixingFraction = hoehenregler;
+     }
   }
-  if(GasMixingFraction > ParamSet.Gas_Max - 20) GasMixingFraction = ParamSet.Gas_Max - 20;
+  if(ThrustMixingFraction > ParamSet.Trust_Max - 20) ThrustMixingFraction = ParamSet.Trust_Max - 20;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  DebugOut.Analog[7] = GasMixingFraction;
+  DebugOut.Analog[7] = ThrustMixingFraction;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier-Anteil
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define MUL_G  1.0
     YawMixingFraction = ReadingYaw - SetPointYaw;     // Regler für Gier
 // YawMixingFraction = 0;
-    if(YawMixingFraction > (GasMixingFraction / 2)) YawMixingFraction = GasMixingFraction / 2;
+    if(YawMixingFraction > (ThrustMixingFraction / 2)) YawMixingFraction = ThrustMixingFraction / 2;
-    if(YawMixingFraction < -(GasMixingFraction / 2)) YawMixingFraction = -(GasMixingFraction / 2);
+    if(YawMixingFraction < -(ThrustMixingFraction / 2)) YawMixingFraction = -(ThrustMixingFraction / 2);
-    if(YawMixingFraction > ((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = ((ParamSet.Gas_Max - GasMixingFraction));
+    if(YawMixingFraction > ((ParamSet.Trust_Max - ThrustMixingFraction))) YawMixingFraction = ((ParamSet.Trust_Max - ThrustMixingFraction));
-    if(YawMixingFraction < -((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = -((ParamSet.Gas_Max - GasMixingFraction));
+    if(YawMixingFraction < -((ParamSet.Trust_Max - ThrustMixingFraction))) YawMixingFraction = -((ParamSet.Trust_Max - ThrustMixingFraction));
-    if(GasMixingFraction < 20) YawMixingFraction = 0;
+    if(ThrustMixingFraction < 20) YawMixingFraction = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Pitch-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffPitch = ReadingPitch - (StickPitch - GPS_Pitch);        // Differenz bestimmen
     if(Gyro_I_Factor) SumPitch += IntegralPitch * Gyro_I_Factor - (StickPitch - GPS_Pitch); // I-Anteil bei Winkelregelung
     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)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64;
+    tmp_int = (long)((long)FCParam.DynamicStability * (long)(ThrustMixingFraction + 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
+    MotorValue = ThrustMixingFraction + pd_ergebnis + YawMixingFraction;          // Mischer
         if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > ParamSet.Gas_Max)              MotorValue = ParamSet.Gas_Max;
+        else if(MotorValue > ParamSet.Trust_Max)            MotorValue = ParamSet.Trust_Max;
-        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
         Motor_Front = MotorValue;
     // Motor Heck
-        MotorValue = GasMixingFraction - pd_ergebnis + YawMixingFraction;
+        MotorValue = ThrustMixingFraction - pd_ergebnis + YawMixingFraction;
         if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > ParamSet.Gas_Max)      MotorValue = ParamSet.Gas_Max;
+        else if(MotorValue > ParamSet.Trust_Max)            MotorValue = ParamSet.Trust_Max;
-        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
         Motor_Rear = MotorValue;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = ReadingRoll - (StickRoll  - GPS_Roll);       // Differenz bestimmen
     if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - (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)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64;
+    tmp_int = (long)((long)FCParam.DynamicStability * (long)(ThrustMixingFraction + 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;
+    MotorValue = ThrustMixingFraction + pd_ergebnis - YawMixingFraction;
 #define GRENZE Poti1
         if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > ParamSet.Gas_Max)          MotorValue = ParamSet.Gas_Max;
+        else if(MotorValue > ParamSet.Trust_Max)                MotorValue = ParamSet.Trust_Max;
-        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
     Motor_Left = MotorValue;
     // Motor Rechts
-        MotorValue = GasMixingFraction - pd_ergebnis - YawMixingFraction;
+        MotorValue = ThrustMixingFraction - pd_ergebnis - YawMixingFraction;

Subversion Repositories FlightCtrl

(root)/branches/V0.68d CRK HexaLotte/fc.c @ 1247 - Rev 706 → 707