WebSVN - FlightCtrl - Diff - Rev 1210 and 1211

 unsigned char TrichterFlug = 0;
 long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 long  ErsatzKompass;
 int   ErsatzKompassInGrad; // Kompasswert in Grad
 int   GierGyroFehler = 0;
-char GyroFaktor;
+char GyroFaktor,GyroFaktorGier;
-char IntegralFaktor;
+char IntegralFaktor,IntegralFaktorGier;
 int  DiffNick,DiffRoll;
 int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links;
 volatile unsigned char Motor1, Motor2,Motor3,Motor4,Motor5,Motor6,Motor7,Motor8;
 volatile unsigned char SenderOkay = 0;
 unsigned int  modell_fliegt = 0;
 volatile unsigned char MikroKopterFlags = 0;
 long GIER_GRAD_FAKTOR = 1291;
 signed int KopplungsteilNickRoll,KopplungsteilRollNick;
 unsigned char RequiredMotors = 4;
-/*
-signed char Mixer.Motor[MAX_MOTORS][4] = {
-                                  {64, +64,   0, +64},//1
-                                  {64, +64, -64, -64},//2
-                                  {64,   0, -64, +64},//3
-                                  {64, -64, -64, -64},//4
-                                  {64, -64,   0, +64},//5
-                                  {64, -64, +64, -64},//6
-                                  {64,   0, +64, +64},//7
-                                  {64, +64, +64, -64},//8
-                                  {  0,   0,   0,   0},//9
-                                  {  0,   0,   0,   0},//10
-                                  {  0,   0,   0,   0},//11
-                                  {  0,   0,   0,   0}};//12
-*/
-/*
-signed char Mixer.Motor[MAX_MOTORS][4] = {
-                                  { 64, +64,   0, +64},//1
-                                  { 64, -64,   0, +64},//2
-                                  { 64,   0, -64, -64},//3
-                                  { 64,   0, +64, -64},//4
-                                  {  0,   0,   0,   0},//5
-                                  {  0,   0,   0,   0},//6
-                                  {  0,   0,   0,   0},//7
-                                  {  0,   0,   0,   0},//8
-                                  {  0,   0,   0,   0},//9
-                                  {  0,   0,   0,   0},//10
-                                  {  0,   0,   0,   0},//11
-                                  {  0,   0,   0,   0}};//12
-*/
 unsigned char Motor[MAX_MOTORS];
 signed int tmp_motorwert[MAX_MOTORS];
 int MotorSmoothing(int neu, int alt)
      static long IntegralFehlerRoll = 0;
          static unsigned int RcLostTimer;
          static unsigned char delay_neutral = 0;
          static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
      static int hoehenregler = 0;
-//       static int motorwert1,motorwert2,motorwert3,motorwert4,motorwert5,motorwert6,motorwert7,motorwert8;
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
      static long ausgleichNick, ausgleichRoll;
      int IntegralNickMalFaktor,IntegralRollMalFaktor;
          unsigned char i;
 // neue Werte von der Funke
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!NewPpmData-- || Notlandung)
-  {
     int tmp_int;
         static int stick_nick,stick_roll;
     ParameterZuordnung();
     stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
     stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
         if(StickGier > 2) StickGier -= 2;       else
         if(StickGier < -2) StickGier += 2; else StickGier = 0;
-        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
-/*   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/STICK_GAIN);
+        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
-    GyroFaktor     = (Parameter_Gyro_P + 10.0);
-//    IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
+    GyroFaktor     = (Parameter_Gyro_P + 10.0);
+    IntegralFaktor = Parameter_Gyro_I;
+    GyroFaktorGier     = (Parameter_Gyro_P + 10.0);
-    IntegralFaktor = Parameter_Gyro_I;
+    IntegralFaktorGier = Parameter_Gyro_I;
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Analoge Steuerung per Seriell
    if(Notlandung)
+    {
      StickGier = 0;
      StickNick = 0;
      StickRoll = 0;
-     GyroFaktor     = 90;//(float) 100 / (256.0 / STICK_GAIN);
+     GyroFaktor     = 90;
-     IntegralFaktor = 120;//(float) 120 / (44000 / STICK_GAIN);
+     IntegralFaktor = 120;
+     GyroFaktorGier     = 90;
+     IntegralFaktorGier = 120;
      Looping_Roll = 0;
      Looping_Nick = 0;
+    }
        fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
        if(abs(MesswertGier) > 128)
+            {
                  fehler = 0;
+                }
-           if(NeueKompassRichtungMerken)
-            {
-//       ErsatzKompass += (fehler * 32) / korrektur;
-//               fehler = 0;
-//               fehler /= 4;
-//               ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
-                }
        if(!KompassSignalSchlecht && w < 25)
+        {
         GierGyroFehler += fehler;
         if(NeueKompassRichtungMerken)
+         {
 #define TRIM_MAX 200
  if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else  if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
- if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
  if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
-    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
-    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
- }
-#ifndef QUADRO
-    MesswertGier =   (long)(MesswertGier * 4 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (4 * (44000 / STICK_GAIN));
+    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
-#else
-    MesswertGier =   (long)(MesswertGier * 2 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (2 * (44000 / STICK_GAIN));
+    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
-#endif
+    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
     // Maximalwerte abfangen
     h = HoehenWert;
     if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
+     {
       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/STICK_GAIN);    // D-Anteil
       h -= (HoeheD)/(8/STICK_GAIN);    // D-Anteil
       tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (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;

Subversion Repositories FlightCtrl

(root)/tags/V0.74d/fc.c @ 2147 - Rev 1210 → 1211