WebSVN - FlightCtrl - Diff - Rev 1153 and 1155 - /branches/V0.80f_ACC-HH

 unsigned int  modell_fliegt = 0;
 unsigned char MikroKopterFlags = 0;
 long GIER_GRAD_FAKTOR = 1291;
 signed int KopplungsteilNickRoll,KopplungsteilRollNick;
 void Piep(unsigned char Anzahl)
+{
  while(Anzahl--)
+ {
      AdNeutralGierBias = AdNeutralGier;
      StartNeutralRoll = AdNeutralRoll;
      StartNeutralNick = AdNeutralNick;
     if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
+    {
-      NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
+      NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
-          NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
+          NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
           NeutralAccZ = Aktuell_az;
+    }
     else
+    {
       NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
     NaviAccNick    += AdWertAccNick;
     NaviAccRoll    += AdWertAccRoll;
     NaviCntAcc++;
     IntegralAccZ  += Aktuell_az - NeutralAccZ;
+if(abs(Mittelwert_AccRoll > 50)) { DebugOut.Analog[16]++; DebugOut.Analog[17] = Mittelwert_AccRoll;};
+//++++++++++++++++++++++++++++++++++++++++++++++++
+// ADC einschalten
+        AdReady = 0;
+    ANALOG_ON;
+//++++++++++++++++++++++++++++++++++++++++++++++++
     if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;
         else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;
         else winkel_roll = Mess_IntegralRoll;
+                         {
               if(AdWertNick > 2000) MesswertNick = +1000;
               if(AdWertNick > 2015) MesswertNick = +2000;
+                         }
-//++++++++++++++++++++++++++++++++++++++++++++++++
-// ADC einschalten
-    ANALOG_ON;
-//++++++++++++++++++++++++++++++++++++++++++++++++
     Integral_Gier  = Mess_Integral_Gier;
     IntegralNick = Mess_IntegralNick;
     IntegralRoll = Mess_IntegralRoll;
     IntegralNick2 = Mess_IntegralNick2;
 //############################################################################
 //
 void MotorRegler(void)
 //############################################################################
+{
-         int motorwert,pd_ergebnis, pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int;
+         int motorwert,pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int;
          int GierMischanteil,GasMischanteil;
      static long SummeNick=0,SummeRoll=0;
      static long sollGier = 0,tmp_long,tmp_long2;
      static long IntegralFehlerNick = 0;
      static long IntegralFehlerRoll = 0;
      if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
+      {
       tmp_long  /= 3;
       tmp_long2 /= 3;
+      }
- #define AUSGLEICH 32
+#define AUSGLEICH  32
       if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
       if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
       if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
       if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
+     }
+     }
+  }
   if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
 #ifdef OCTO
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     pd_ergebnis_roll = DiffRoll + SummeRoll / Ki;       // PI-Regler für Roll
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
     if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
+#ifndef OCTO
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Quadro-Mischer
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    motorwert = GasMischanteil + pd_ergebnis_nick + GierMischanteil;      // Mischer
+    motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
+    Motor_Vorne = motorwert;
+        motorwert = GasMischanteil - pd_ergebnis_nick + GierMischanteil;
+    motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
+        Motor_Hinten = motorwert;
+    motorwert = GasMischanteil + pd_ergebnis_roll - GierMischanteil;
+    motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
+    Motor_Links = motorwert;
+        motorwert = GasMischanteil - pd_ergebnis_roll - GierMischanteil;
+    motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
+    Motor_Rechts = motorwert;
+   // +++++++++++++++++++++++++++++++++++++++++++++++
 #else
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Octo-Mischer
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil;
-    motorwert /= STICK_GAIN;
-        if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
+    motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
         Motor1 = motorwert;
-    motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil;
-    motorwert /= STICK_GAIN;
-        if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
+    motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
-        Motor2 = motorwert;
-        motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil;
-    motorwert /= STICK_GAIN;
+        Motor2 = motorwert;
         if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
-    Motor3 = motorwert;
-        motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil;
+    motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
-    motorwert /= STICK_GAIN;
+    Motor3 = motorwert;
         if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
-    Motor4 = motorwert;
+        motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil;
-        motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil;
+    motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
-    motorwert /= STICK_GAIN;
+    Motor4 = motorwert;
-        if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
-        Motor5 = motorwert;
         motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil;
-        motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil;
+    motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
-    motorwert /= STICK_GAIN;
-        if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        Motor5 = motorwert;
         Motor6 = motorwert;
         motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil;
-    motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil;
-    motorwert /= STICK_GAIN;
-        if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+    motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        Motor6 = motorwert;
     Motor7 = motorwert;
+    motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil;
     motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
-    motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil;
+    Motor7 = motorwert;
     motorwert /= STICK_GAIN;
-        if ((motorwert < 0)) motorwert = 0;
+    motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil;
-        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+    motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS);
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    Motor8 = motorwert;
-    Motor8 = motorwert;
+   // +++++++++++++++++++++++++++++++++++++++++++++++
-   // +++++++++++++++++++++++++++++++++++++++++++++++
-/*
-if(Poti1 > 20)  Motor1 = 0;
-if(Poti1 > 90)  Motor6 = 0;
-if(Poti1 > 140) Motor2 = 0;
-if(Poti1 > 200) Motor7 = 0;
-*/
-}
-#else
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Mischer und PI-Regler
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  DebugOut.Analog[7] = GasMischanteil;
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Gier-Anteil
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-#define MUL_G  1.0
-    GierMischanteil = MesswertGier - sollGier * STICK_GAIN;     // Regler für Gier
-//GierMischanteil = 0;
-#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 > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
-    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Nick-Achse
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
-    if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
-    else  SummeNick += DiffNick; // I-Anteil bei HH
-    if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
-    if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
-    pd_ergebnis = DiffNick + SummeNick / Ki; // 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;
-    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 Heck
-        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_Hinten = motorwert;
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Roll-Achse
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
-    if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
-    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
-    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
-    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
-    pd_ergebnis = DiffRoll + SummeRoll / Ki;    // PI-Regler für Roll
-    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.80f_ACC-HH_MartinR/fc.c @ 1728 - Rev 1153 → 1155