WebSVN - FlightCtrl - Diff - Rev 854 and 855 - /branches/V0.76g

 volatile long Mess_Integral_Hoch = 0;
 volatile int  KompassValue = 0;
 volatile int  KompassStartwert = 0;
 volatile int  KompassRichtung = 0;
 unsigned int  KompassSignalSchlecht = 500;
-unsigned char MAX_GAS,MIN_GAS;
+unsigned char  MAX_GAS,MIN_GAS;
 unsigned char Notlandung = 0;
 unsigned char HoehenReglerAktiv = 0;
 long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 long  ErsatzKompass;
 int   ErsatzKompassInGrad; // Kompasswert in Grad
 /*   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick)
      MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--;
    if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll)
      MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;
 */
-    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
+    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / (256.0/STICK_GAIN);
-    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
+    IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Digitale Steuerung per DubWise
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(DubWiseKeys[0] & 8)  ExternStickGier = 50;else
     if(DubWiseKeys[0] & 4)  ExternStickGier =-50;else ExternStickGier = 0;
     if(DubWiseKeys[0] & 2)  ExternHoehenValue++;
     if(DubWiseKeys[0] & 16) ExternHoehenValue--;
-    StickNick += ExternStickNick / 8;
+    StickNick += (STICK_GAIN * ExternStickNick) / 8;
-    StickRoll += ExternStickRoll / 8;
+    StickRoll += (STICK_GAIN * ExternStickRoll) / 8;
-    StickGier += ExternStickGier;
+    StickGier += STICK_GAIN * ExternStickGier;
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Analoge Steuerung per Seriell
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)
          StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
          StickGier += ExternControl.Gier;
      ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
      if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
+    }
+    if(StickGas < 0) StickGas = 0;
     if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
     if(GyroFaktor < 0) GyroFaktor = 0;
     if(IntegralFaktor < 0) IntegralFaktor = 0;
-    if(abs(StickNick) > MaxStickNick) MaxStickNick = abs(StickNick); else MaxStickNick--;
+    if(abs(StickNick/STICK_GAIN) > MaxStickNick) MaxStickNick = abs(StickNick); else MaxStickNick--;
-    if(abs(StickRoll) > MaxStickRoll) MaxStickRoll = abs(StickRoll); else MaxStickRoll--;
+    if(abs(StickRoll/STICK_GAIN) > MaxStickRoll) MaxStickRoll = abs(StickRoll); else MaxStickRoll--;
     if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}
    if(Notlandung)
+    {
      StickGier = 0;
      StickNick = 0;
      StickRoll = 0;
-     GyroFaktor  = 0.1;
+     GyroFaktor     = (float) 100 / (256.0 / STICK_GAIN);
-     IntegralFaktor = 0.005;
+     IntegralFaktor = (float) 120 / (44000 / STICK_GAIN);
      Looping_Roll = 0;
      Looping_Nick = 0;
+    }
     if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
     else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
     if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
     else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
-    MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
+    MesswertGier =   MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
     DebugOut.Analog[21] = MesswertNick;
 // Höhenregelung
 // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //OCR0B = 180 - (Poti1 + 120) / 4;
 //DruckOffsetSetting = OCR0B;
+  GasMischanteil *= STICK_GAIN;
  if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
+  {
     int tmp_int;
     if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
+    {
     if(Notlandung) SollHoehe = 0;
     h = HoehenWert;
     if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
      {
-      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
+      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
       h = GasMischanteil - h;         // vom Gas abziehen
-      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
+      h -= (HoeheD * Parameter_Luftdruck_D)/(8/STICK_GAIN);    // D-Anteil
-      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
+      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (32 / STICK_GAIN);
-      if(tmp_int > 50) tmp_int = 50;
+      if(tmp_int > 50*STICK_GAIN) tmp_int = 50*STICK_GAIN;
-      else if(tmp_int < -50) tmp_int = -50;
+      else if(tmp_int < -(50*STICK_GAIN)) tmp_int = -(50*STICK_GAIN);
       h -= tmp_int;
       hoehenregler = (hoehenregler*15 + h) / 16;
-      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
+      if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
        {
-         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
+         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
-         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
+         if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
+       }
       if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
       GasMischanteil = hoehenregler;
+     }
   }
-  if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
+  if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   DebugOut.Analog[7] = GasMischanteil;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier-Anteil
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define MUL_G  1.0
-    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
+    GierMischanteil = MesswertGier - sollGier * STICK_GAIN;     // Regler für Gier
 // GierMischanteil = 0;
-#define MIN_GIERGAS  35  // unter diesem Gaswert trotzdem Gieren
+#define MIN_GIERGAS  (40*STICK_GAIN)  // unter diesem Gaswert trotzdem Gieren
    if(GasMischanteil > MIN_GIERGAS)
+    {
      if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
      if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
+    }
     else
+    {
      if(GierMischanteil > (MIN_GIERGAS / 2))  GierMischanteil = MIN_GIERGAS / 2;
      if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
+    }
-    }
+    tmp_int = MAX_GAS*STICK_GAIN;
-    if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil));
+    if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
-    if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS - GasMischanteil));
+    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
     if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
     else  SummeNick += DiffNick; // I-Anteil bei HH
-    if(SummeNick >  16000) SummeNick =  16000;
+    if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
-    if(SummeNick < -16000) SummeNick = -16000;
+    if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
     pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
     // Motor Vorn
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
+    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
     if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
     motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
-    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
+    motorwert /= STICK_GAIN;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        Motor_Vorne = motorwert;
-        Motor_Vorne = motorwert;
+    // Motor Heck
-    // Motor Heck
+        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
-        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
+    motorwert /= STICK_GAIN;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
     if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
     // Motor Links
     motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
+    motorwert /= STICK_GAIN;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
     Motor_Links = motorwert;
     // Motor Rechts
         motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
+    motorwert /= STICK_GAIN;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
     Motor_Rechts = motorwert;
    // +++++++++++++++++++++++++++++++++++++++++++++++

Subversion Repositories FlightCtrl

(root)/branches/V0.76g_VarMotorSmoothing/fc.c @ 1409 - Rev 854 → 855