WebSVN - FlightCtrl - Diff - Rev 918 and 921 - /branches/V0.80g_ACC-HH_MartinR

 unsigned char Parameter_ServoNickControl = 100;
 unsigned char Parameter_LoopGasLimit = 70;
 unsigned char Parameter_AchsKopplung1 = 0;
 unsigned char Parameter_AchsGegenKopplung1 = 0;
 unsigned char Parameter_DynamicStability = 100;
+unsigned char Parameter_J16Bitmask;             // for the J16 Output
+unsigned char Parameter_J16Timing;              // for the J16 Output
+unsigned char Parameter_J17Bitmask;             // for the J17 Output
+unsigned char Parameter_J17Timing;              // for the J17 Output
+unsigned char Parameter_NaviGpsModeControl;     // Parameters for the Naviboard
+unsigned char Parameter_NaviGpsGain;
+unsigned char Parameter_NaviGpsP;
+unsigned char Parameter_NaviGpsI;
+unsigned char Parameter_NaviGpsD;
+unsigned char Parameter_NaviGpsACC;
+unsigned char Parameter_ExternalControl;
 struct mk_param_struct EE_Parameter;
 signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 int MaxStickNick = 0,MaxStickRoll = 0;
 unsigned int  modell_fliegt = 0;
+unsigned char MikroKopterFlags = 0;
 void Piep(unsigned char Anzahl)
+{
  while(Anzahl--)
         Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
     ExternHoehenValue = 0;
     ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
     GierGyroFehler = 0;
     SendVersionToNavi = 1;
+    LED_Init();
+    MikroKopterFlags |= FLAG_CALIBRATE;
+}
 //############################################################################
 // Bearbeitet die Messwerte
 //############################################################################
 // Senden der Motorwerte per I2C-Bus
 void SendMotorData(void)
 //############################################################################
+{
-    if(MOTOR_OFF || !MotorenEin)
+    if(!MotorenEin)
+        {
         Motor_Hinten = 0;
         Motor_Vorne = 0;
         Motor_Rechts = 0;
         Motor_Links = 0;
         if(MotorTest[0]) Motor_Vorne = MotorTest[0];
         if(MotorTest[1]) Motor_Hinten = MotorTest[1];
         if(MotorTest[2]) Motor_Links = MotorTest[2];
         if(MotorTest[3]) Motor_Rechts = MotorTest[3];
+        MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
-        }
+        } else MikroKopterFlags |= FLAG_MOTOR_RUN;
     DebugOut.Analog[12] = Motor_Vorne;
     DebugOut.Analog[13] = Motor_Hinten;
     DebugOut.Analog[14] = Motor_Links;
 // Trägt ggf. das Poti als Parameter ein
 void ParameterZuordnung(void)
 //############################################################################
+{
+ #define CHK_POTI_MM(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;}
- #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;}
+ #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; }
  CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
- CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
+ CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
- CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
+ CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
  CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
  CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
- CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
+ CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
  CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
  CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
  CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
  CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
  CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
  CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
  CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
  CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
  CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
+ CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
+ CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
+ CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
+ CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
+ CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
+ CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
+ CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
+ CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
+ CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);
  Ki = (float) Parameter_I_Faktor * 0.0001;
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(SenderOkay > 140)
+            {
             Notlandung = 0;
             RcLostTimer = EE_Parameter.NotGasZeit * 50;
-            if(GasMischanteil > 40)
+            if(GasMischanteil > 40 && MotorenEin)
+                {
                 if(modell_fliegt < 0xffff) modell_fliegt++;
+                }
             if((modell_fliegt < 256))
+                {
                   NeueKompassRichtungMerken = 1;
                   sollGier = 0;
                   Mess_Integral_Gier = 0;
                   Mess_Integral_Gier2 = 0;
+                 }
-                }
+                } else MikroKopterFlags |= FLAG_FLY;
             if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // auf Nullwerte kalibrieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         Mess_IntegralRoll = 0;
                         Mess_IntegralNick2 = IntegralNick;
                         Mess_IntegralRoll2 = IntegralRoll;
                         SummeNick = 0;
                         SummeRoll = 0;
+                        MikroKopterFlags |= FLAG_START;
+                        }
+                    }
                     else delay_einschalten = 0;
                 //Auf Neutralwerte setzen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Digitale Steuerung per DubWise
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-#define KEY_VALUE (Parameter_UserParam8 * 4)  //(Poti3 * 8)
+#define KEY_VALUE (Parameter_ExternalControl * 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;
     ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
     StickRoll += (STICK_GAIN * ExternStickRoll) / 8;
     StickGier += STICK_GAIN * ExternStickGier;
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Analoge Steuerung per Seriell
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-   if(ExternControl.Config & 0x01 && Parameter_UserParam8 > 128)
+   if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128)
+    {
          StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
          StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
          StickGier += ExternControl.Gier;
      ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
 //    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
     if(GierGyroFehler > ABGLEICH_ANZAHL/2) AdNeutralGier++;
     if(GierGyroFehler <-ABGLEICH_ANZAHL/2) AdNeutralGier--;
-DebugOut.Analog[22] = MittelIntegralRoll / 26;
+DebugOut.Analog[22] = MittelIntegralRoll / 26;
 //DebugOut.Analog[24] = GierGyroFehler;
     GierGyroFehler = 0;
          } else  last_n_n = 0;
+        }
         else
+        {
          cnt = 0;
-         KompassSignalSchlecht = 500;
+         KompassSignalSchlecht = 1000;
+        }
         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
         if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
         if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
            else last_r_n = 1;
          } else  last_r_n = 0;
         } else
+        {
          cnt = 0;
-         KompassSignalSchlecht = 500;
+         KompassSignalSchlecht = 1000;
+        }
         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
         if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
-        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
-/*DebugOut.Analog[27] = ausgleichRoll;
-DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
-DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
-*/
+        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
+  }
   else
+  {
    LageKorrekturRoll = 0;
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //    if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
     if(abs(StickGier) > 15) // war 35
+     {
+      KompassSignalSchlecht = 1000;
       if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
+       {
          NeueKompassRichtungMerken = 1;
          KompassStartwert = ErsatzKompass;
-         KompassSignalSchlecht = 250;
         };
+     }
     tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
     tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
     sollGier = tmp_int;
        int w,v,r,fehler,korrektur;
        w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
        v = abs(IntegralRoll /512);
        if(v > w) w = v; // grösste Neigung ermitteln
        korrektur = w / 8 + 1;
+       fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
+//DebugOut.Analog[25] = KompassSignalSchlecht;
+       if(!KompassSignalSchlecht && w < 25)
+        {
+        GierGyroFehler += fehler;
-       if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht)
+        if(NeueKompassRichtungMerken)
-        {
+         {
-         beeptime = 200;
+          beeptime = 200;
 //         KompassStartwert = KompassValue;
-         KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
+          KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
-         NeueKompassRichtungMerken = 0;
+          NeueKompassRichtungMerken = 0;
+         }
+        }
-       fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
        ErsatzKompass += (fehler * 8) / korrektur;
-       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
+       w = (w * Parameter_KompassWirkung) / 32;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
-       if(w > 0)
+       if(w >= 0)
+        {
           if(!KompassSignalSchlecht)
+          {
-           GierGyroFehler += fehler;
            v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
            r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
 //           r = KompassRichtung;
            v = (r * w) / v;  // nach Kompass ausrichten
            w = 3 * Parameter_KompassWirkung;
            if(v < -w) v = -w;
            Mess_Integral_Gier += v;
+          }
           if(KompassSignalSchlecht) KompassSignalSchlecht--;
+        }
-        else KompassSignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 0,5 sek
+        else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
+     }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Subversion Repositories FlightCtrl

(root)/branches/V0.80g_ACC-HH_MartinR_HR/fc.c @ 1993 - Rev 918 → 921