WebSVN - FlightCtrl - Diff - Rev 304 and 395 - /branches/V0.80g_ACC-HH_MartinR

 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+#include "main.h"
-#include "main.h"
+#include "eeprom.c"
 unsigned char h,m,s;
 volatile unsigned int I2CTimeout = 100;
 volatile int MesswertNick,MesswertRoll,MesswertGier;
 volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
 volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 volatile float NeutralAccZ = 0;
 unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
+volatile long IntegralNick = 0,IntegralNick2 = 0;
-volatile long IntegralNick = 0,IntegralNick2 = 0;
+volatile long IntegralRoll = 0,IntegralRoll2 = 0;
-volatile long IntegralRoll = 0,IntegralRoll2 = 0;
+volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
 volatile long Integral_Gier = 0;
 volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
+volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
-volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
+volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
-volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
+volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
 volatile long Mess_Integral_Hoch = 0;
 volatile int  KompassValue = 0;
 volatile int  KompassStartwert = 0;
 volatile int  KompassRichtung = 0;
 unsigned char MAX_GAS,MIN_GAS;
+unsigned char Notlandung = 0;
-unsigned char Notlandung = 0;
+unsigned char HoehenReglerAktiv = 0;
-unsigned char HoehenReglerAktiv = 0;
+long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 volatile unsigned char SenderOkay = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0;
 char MotorenEin = 0;
 int HoehenWert = 0;
 int SollHoehe = 0;
-unsigned char Looping_Nick = 0,Looping_Roll = 0;
+int I_LageRoll = 0,I_LageNick = 0;
 float Kp =  FAKTOR_P;
 float Ki =  FAKTOR_I;
+unsigned char Looping_Nick = 0,Looping_Roll = 0;
+unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
 unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
 unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
 unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
 unsigned char Parameter_UserParam2 = 0;
 unsigned char Parameter_UserParam3 = 0;
 unsigned char Parameter_UserParam4 = 0;
 unsigned char Parameter_ServoNickControl = 100;
 unsigned char Parameter_LoopGasLimit = 70;
+unsigned char Parameter_AchsKopplung1 = 0;
+unsigned char Parameter_AchsGegenKopplung1 = 0;
 struct mk_param_struct EE_Parameter;
 void Piep(unsigned char Anzahl)
+{
 //############################################################################
 //  Nullwerte ermitteln
 void SetNeutral(void)
 //############################################################################
+{
-    unsigned int timer;
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
         AdNeutralGier = 0;
+    Parameter_AchsKopplung1 = 0;
+    Parameter_AchsGegenKopplung1 = 0;
     CalibrierMittelwert();
-    timer = SetDelay(5);
+    Delay_ms_Mess(100);
-        while (!CheckDelay(timer));
         CalibrierMittelwert();
     if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+     {
       if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+     }
-    if(PlatinenVersion == 10)
-    {
-     AdNeutralNick= abs(MesswertNick);
+     AdNeutralNick= AdWertNick;
-         AdNeutralRoll= abs(MesswertRoll);
+         AdNeutralRoll= AdWertRoll;
-         AdNeutralGier= abs(MesswertGier);
-    }
-    else
-    {
-     AdNeutralNick= abs(MesswertNick) / 2;
-         AdNeutralRoll= abs(MesswertRoll) / 2;
-         AdNeutralGier= abs(MesswertGier) / 2;
+         AdNeutralGier= AdWertGier;
     NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
         NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
         NeutralAccZ = Aktuell_az;
     HoeheD = 0;
     Mess_Integral_Hoch = 0;
     KompassStartwert = KompassValue;
     GPS_Neutral();
     beeptime = 50;
+        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
+        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
+}
 //############################################################################
-// Bildet den Mittelwert aus den Messwerten
+// Bearbeitet die Messwerte
 void Mittelwert(void)
 //############################################################################
+{
-{
+    static signed long tmpl,tmpl2;
-    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
+    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
-    ANALOG_OFF;
+    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
+    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
+    if(PlatinenVersion != 10)
+     {
-        if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
+      MesswertGier *= 2;
+          MesswertRoll *= 2;
+      MesswertNick *= 2;
-        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
+     }
-        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
+// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
-        if(messanzahl_AccNick) Mittelwert_AccNick = ((long)Mittelwert_AccNick * 7 + ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick)) / 8L;
+        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
-        if(messanzahl_AccRoll) Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 7 + ((ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll)) / 8L;
+        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
+        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
+    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
+    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
+    IntegralAccZ    += Aktuell_az - 704;//NeutralAccZ;
+// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
+            Mess_Integral_Gier +=  MesswertGier;
+            Mess_Integral_Gier2 += MesswertGier;
+// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
+      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
-        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 7 + ((long)accumulate_AccHoch) / messanzahl_AccHoch) / 8L;
+         {
+            tmpl = Mess_IntegralNick / 4096L;
+            tmpl *= MesswertGier;
+            tmpl *= Parameter_AchsKopplung1;  //125
+            tmpl /= 2048L;
+            tmpl2 = Mess_IntegralRoll / 4096L;
+            tmpl2 *= MesswertGier;
+            tmpl2 *= Parameter_AchsKopplung1;
+            tmpl2 /= 2048L;
+         }
+      else  tmpl = tmpl2 = 0;
+// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
+            MesswertRoll += tmpl;
+            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
+            Mess_IntegralRoll2 += MesswertRoll;
+            Mess_IntegralRoll +=  MesswertRoll - I_LageRoll;
+            if(Mess_IntegralRoll > Umschlag180Roll)
+            {
+             Mess_IntegralRoll  = -(Umschlag180Roll - 10000L);
+             Mess_IntegralRoll2 = Mess_IntegralRoll;
+            }
+            if(Mess_IntegralRoll <-Umschlag180Roll)
+            {
+             Mess_IntegralRoll =  (Umschlag180Roll - 10000L);
+             Mess_IntegralRoll2 = Mess_IntegralRoll;
-    AccumulateNick = 0;   MessanzahlNick = 0;
+            }
-    AccumulateRoll = 0;   MessanzahlRoll = 0;
+            if(AdWertRoll < 15)   MesswertRoll = -1000;
+            if(AdWertRoll <  7)   MesswertRoll = -2000;
-    AccumulateGier = 0;   MessanzahlGier = 0;
+            if(AdWertRoll > 1010) MesswertRoll = +1000;
+            if(AdWertRoll > 1017) MesswertRoll = +2000;
+// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
+            MesswertNick -= tmpl2;
+            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
+            Mess_IntegralNick2 += MesswertNick;
+            Mess_IntegralNick  += MesswertNick - I_LageNick;
+            if(Mess_IntegralNick > Umschlag180Nick)
+            {
+             Mess_IntegralNick = -(Umschlag180Nick - 10000L);
+             Mess_IntegralNick2 = Mess_IntegralNick;
+            }
+            if(Mess_IntegralNick <-Umschlag180Nick)
+            {
+             Mess_IntegralNick =  (Umschlag180Nick - 10000L);
+             Mess_IntegralNick2 = Mess_IntegralNick;
+            }
+            if(AdWertNick < 15)   MesswertNick = -1000;
-    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
+            if(AdWertNick <  7)   MesswertNick = -2000;
-    accumulate_AccNick = 0;messanzahl_AccNick = 0;
+            if(AdWertNick > 1010) MesswertNick = +1000;
+            if(AdWertNick > 1017) MesswertNick = +2000;
+//++++++++++++++++++++++++++++++++++++++++++++++++
+// ADC einschalten
+    ANALOG_ON;
+//++++++++++++++++++++++++++++++++++++++++++++++++
     accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
-    Integral_Gier  = Mess_Integral_Gier;
-//    Integral_Gier2 = Mess_Integral_Gier2;
+    Integral_Gier  = Mess_Integral_Gier;
     IntegralNick = Mess_IntegralNick;
     IntegralRoll = Mess_IntegralRoll;
     IntegralNick2 = Mess_IntegralNick2;
-    IntegralRoll2 = Mess_IntegralRoll2;
-    // ADC einschalten
+    IntegralRoll2 = Mess_IntegralRoll2;
-    ANALOG_ON;
-/*
+  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
-//------------------------------------------------------------------------------
-    if(MesswertNick > 200)  MesswertNick += 4 * (MesswertNick - 200);
+  {
-    else
+    if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);
-    if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
-    if(MesswertRoll > 200)  MesswertRoll += 4 * (MesswertRoll - 200);
+    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
-    else
-    if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
+    if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);
-//------------------------------------------------------------------------------
+    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
 */
     if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
     if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
     if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 void CalibrierMittelwert(void)
 //############################################################################
+{
     // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
         ANALOG_OFF;
-    if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
-        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
-        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
-        if(messanzahl_AccNick) Mittelwert_AccNick = ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick);
-        if(messanzahl_AccRoll) Mittelwert_AccRoll = (ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll;
-        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)accumulate_AccHoch) / messanzahl_AccHoch;
-    AccumulateNick = 0;   MessanzahlNick = 0;
+        MesswertNick = AdWertNick;
-    AccumulateRoll = 0;   MessanzahlRoll = 0;
+        MesswertRoll = AdWertRoll;
-    AccumulateGier = 0;   MessanzahlGier = 0;
+        MesswertGier = AdWertGier;
-    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
+        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
-    accumulate_AccNick = 0;messanzahl_AccNick = 0;
+        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
-    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
+        Mittelwert_AccHoch = (long)AdWertAccHoch;
-    // ADC einschalten
+   // ADC einschalten
     ANALOG_ON;
     if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
     if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
     if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
     if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
     if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
     if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
     if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
     if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
+        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
+        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
+}
 //############################################################################
 // Senden der Motorwerte per I2C-Bus
     twi_state = 0;
     motor = 0;
     i2c_start();
+}
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Konstanten
-// + 0-250 -> normale Werte
-// + 251 -> Poti1
-// + 252 -> Poti2
-// + 253 -> Poti3
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-void DefaultKonstanten1(void)
-{
- EE_Parameter.Kanalbelegung[K_NICK]  = 1;
- EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
- EE_Parameter.Kanalbelegung[K_GAS]   = 3;
- EE_Parameter.Kanalbelegung[K_GIER]  = 4;
- EE_Parameter.Kanalbelegung[K_POTI1] = 5;
- EE_Parameter.Kanalbelegung[K_POTI2] = 6;
- EE_Parameter.Kanalbelegung[K_POTI3] = 7;
- EE_Parameter.Kanalbelegung[K_POTI4] = 8;
- EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;
- EE_Parameter.Hoehe_MinGas = 30;
- EE_Parameter.MaxHoehe     = 251;      // Wert : 0-32   251 -> Poti1
- EE_Parameter.Hoehe_P      = 10;       // Wert : 0-32
- EE_Parameter.Luftdruck_D  = 50;       // Wert : 0-250
- EE_Parameter.Hoehe_ACC_Wirkung = 50;  // Wert : 0-250
- EE_Parameter.Hoehe_Verstaerkung = 4;  // Wert : 0-50
- EE_Parameter.Stick_P = 4; //2         // Wert : 1-6
- EE_Parameter.Stick_D = 8; //8         // Wert : 0-64
- EE_Parameter.Gier_P = 16;             // Wert : 1-20
- EE_Parameter.Gas_Min = 15;            // Wert : 0-32
- EE_Parameter.Gas_Max = 250;           // Wert : 33-250
- EE_Parameter.GyroAccFaktor = 26;      // Wert : 1-64
- EE_Parameter.KompassWirkung = 128;    // Wert : 0-250
- EE_Parameter.Gyro_P = 120; //80          // Wert : 0-250
- EE_Parameter.Gyro_I = 150;               // Wert : 0-250
- EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250
- EE_Parameter.NotGas = 35;                // Wert : 0-250     // Gaswert bei Empangsverlust
- EE_Parameter.NotGasZeit = 20;            // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
- EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
- EE_Parameter.I_Faktor = 0;
- EE_Parameter.UserParam1 = 0;             //zur freien Verwendung
- EE_Parameter.UserParam2 = 0;             //zur freien Verwendung
- EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
- EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
- EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
- EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
- EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
- EE_Parameter.ServoNickMin = 50;          // Wert : 0-250     // Anschlag
- EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
- EE_Parameter.ServoNickRefresh = 5;
- EE_Parameter.LoopGasLimit = 50;
- EE_Parameter.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
- EE_Parameter.LoopConfig = 0;             // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt
- memcpy(EE_Parameter.Name, "Sport\0", 12);
-}
-void DefaultKonstanten2(void)
-{
- EE_Parameter.Kanalbelegung[K_NICK]  = 1;
- EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
- EE_Parameter.Kanalbelegung[K_GAS]   = 3;
- EE_Parameter.Kanalbelegung[K_GIER]  = 4;
- EE_Parameter.Kanalbelegung[K_POTI1] = 5;
- EE_Parameter.Kanalbelegung[K_POTI2] = 6;
- EE_Parameter.Kanalbelegung[K_POTI3] = 7;
- EE_Parameter.Kanalbelegung[K_POTI4] = 8;
- EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;
- EE_Parameter.Hoehe_MinGas = 30;
- EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
- EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
- EE_Parameter.Luftdruck_D  = 50;      // Wert : 0-250
- EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250
- EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
- EE_Parameter.Stick_P = 4; //2           // Wert : 1-6
- EE_Parameter.Stick_D = 0; //8           // Wert : 0-64
- EE_Parameter.Gier_P = 16;            // Wert : 1-20
- EE_Parameter.Gas_Min = 15;            // Wert : 0-32
- EE_Parameter.Gas_Max = 250;          // Wert : 33-250
- EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
- EE_Parameter.KompassWirkung = 128;    // Wert : 0-250
- EE_Parameter.Gyro_P = 175; //80           // Wert : 0-250
- EE_Parameter.Gyro_I = 175;           // Wert : 0-250
- EE_Parameter.UnterspannungsWarnung = 94;  // Wert : 0-250
- EE_Parameter.NotGas = 35;                 // Wert : 0-250     // Gaswert bei Empangsverlust
- EE_Parameter.NotGasZeit = 20;             // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
- EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
- EE_Parameter.I_Faktor = 0;
- EE_Parameter.UserParam1 = 0;   //zur freien Verwendung
- EE_Parameter.UserParam2 = 0;   //zur freien Verwendung
- EE_Parameter.UserParam3 = 0;   //zur freien Verwendung
- EE_Parameter.UserParam4 = 0;   //zur freien Verwendung
- EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
- EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
- EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
- EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
- EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
- EE_Parameter.ServoNickRefresh = 5;
- EE_Parameter.LoopGasLimit = 50;
- EE_Parameter.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
- EE_Parameter.LoopConfig = 0;              // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
- memcpy(EE_Parameter.Name, "Kamera\0", 12);
  CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
  CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
  CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
  CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
  CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
- CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,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);
  Ki = (float) Parameter_I_Faktor * 0.0001;
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
+{
          int motorwert,pd_ergebnis,h,tmp_int;
          int GierMischanteil,GasMischanteil;
      static long SummeNick=0,SummeRoll=0;
      static long sollGier = 0,tmp_long,tmp_long2;
-     long IntegralFehlerNick = 0;
+     static long IntegralFehlerNick = 0;
-     long IntegralFehlerRoll = 0;
+     static long IntegralFehlerRoll = 0;
          static unsigned int RcLostTimer;
          static unsigned char delay_neutral = 0;
          static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
          static unsigned int  modell_fliegt = 0;
      static int hoehenregler = 0;
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
+     static long ausgleichNick, ausgleichRoll;
         Mittelwert();
-    GRN_ON;
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // auf Nullwerte kalibrieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
+                    {
+                    unsigned char setting = 2;
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                         GRN_OFF;
                         MotorenEin = 0;
                         delay_neutral = 0;
+      {
        StickRoll += 8 * EE_Parameter.Stick_P;
        if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;
+      }
+     }
-  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  if(((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) ||
+  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
-     ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
+  else
+   {
-    Looping_Roll = 1;
+     {
-    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
+      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;
+     }
    }
+  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
+   else
+   {
-   else Looping_Roll = 0;
+   if(Looping_Rechts) // Hysterese
+     {
+      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
+     }
+   }
-  if(((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) ||
+  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
-     ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN))
+  else
    {
+    if(Looping_Oben)  // Hysterese
+     {
+      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;
+     }
+   }
+  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
+   else
+   {
+    if(Looping_Unten) // Hysterese
+     {
+      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
+     }
+   }
+   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
+   if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
+  } // Ende neue Funken-Werte
-    Looping_Nick = 1;
+  if(Looping_Roll) beeptime = 100;
+  if(Looping_Roll || Looping_Nick)
     Looping_Roll = 0;
     if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
-   }
    else Looping_Nick = 0;
      GyroFaktor  = 0.1;
      IntegralFaktor = 0.005;
      Looping_Roll = 0;
      Looping_Nick = 0;
+    }
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Gyro-Drift kompensieren
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-#define DRIFT_FAKTOR 3
-    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
-        {
-        IntegralFehlerNick = IntegralNick2 - IntegralNick;
-        IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
-        ZaehlMessungen = 0;
-        if(IntegralFehlerNick > 800/DRIFT_FAKTOR)   AdNeutralNick++;
-        if(IntegralFehlerNick < -800/DRIFT_FAKTOR)  AdNeutralNick--;
-        if(IntegralFehlerRoll > 800/DRIFT_FAKTOR)   AdNeutralRoll++;
-        if(IntegralFehlerRoll < -800/DRIFT_FAKTOR)  AdNeutralRoll--;
-//        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)
-//        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;   // macht nur mit Referenz (Kompass Sinn)
-    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
-        Mess_IntegralNick2 = IntegralNick;
-        Mess_IntegralRoll2 = IntegralRoll;
-        Mess_Integral_Gier2 = Integral_Gier;
-    ANALOG_ON;  // ADC einschalten
-        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Integrale auf ACC-Signal abgleichen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+#define ABGLEICH_ANZAHL 256L
+ MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
+ MittelIntegralRoll  += IntegralRoll;
+ MittelIntegralNick2 += IntegralNick2;
+ MittelIntegralRoll2 += IntegralRoll2;
-  if(!Looping_Nick && !Looping_Roll)
+ if(Looping_Nick || Looping_Roll)
+  {
-    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
+    IntegralAccNick = 0;
-    tmp_long /= 32;
+    IntegralAccRoll = 0;
-    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
+    MittelIntegralNick = 0;
-    tmp_long2 /= 32;
+    MittelIntegralRoll = 0;
- #define AUSGLEICH 100
+    MittelIntegralNick2 = 0;
-    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
+    MittelIntegralRoll2 = 0;
-    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
+    Mess_IntegralNick2 = Mess_IntegralNick;
-    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
+    Mess_IntegralRoll2 = Mess_IntegralRoll;
-    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
+    ZaehlMessungen = 0;
+    I_LageNick = 0;
+    I_LageRoll = 0;
+  }
+ if(ZaehlMessungen >= ABGLEICH_ANZAHL)
-  else
+ {
+  static int cnt = 0;
+  static char last_n_p,last_n_n,last_r_p,last_r_n;
+  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
+  if(!Looping_Nick && !Looping_Roll)
+  {
+    MittelIntegralNick  /= ABGLEICH_ANZAHL;
+    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
+        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
+        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
+    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
+//DebugOut.Analog[6] = labs(MittelIntegralNick_Alt - MittelIntegralNick) / 10;
+// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
+    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
+    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
+    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)
+     {
+       tmp_long = IntegralFehlerNick / 700L;
+       if(tmp_long > 40) tmp_long = 40;
+       if(tmp_long <-40) tmp_long =-40;
+       I_LageNick = tmp_long;
+     }
+     else
+     {
+      I_LageNick /= 2;
+     }
+// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
+    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
+    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
+    if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000)
+     {
+      tmp_long2 = IntegralFehlerRoll / 700L;
+      if(tmp_long2 > 40) tmp_long2 = 40;
+      if(tmp_long2 <-40) tmp_long2 =-40;
+      I_LageRoll = tmp_long2;
+     }
+     else
+     {
+      I_LageRoll /=2;
+     }
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++
+   MittelIntegralNick_Alt = MittelIntegralNick;
+   MittelIntegralRoll_Alt = MittelIntegralRoll;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++
+    Mess_IntegralNick -= ausgleichNick;
+    Mess_IntegralRoll -= ausgleichRoll;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Gyro-Drift ermitteln
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
+    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
+    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
+    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
+    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
+    IntegralFehlerNick = tmp_long;
+    IntegralFehlerRoll = tmp_long2;
+    Mess_IntegralNick2 -= IntegralFehlerNick;
+    Mess_IntegralRoll2 -= IntegralFehlerRoll;
+    if(PlatinenVersion != 10)
+    {
-   tmp_long = 0;
+     tmp_long /= 2;
-   tmp_long2 = 0;
+     tmp_long2 /= 2;
+    }
+//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
+//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
+DebugOut.Analog[30] = I_LageRoll;
+DebugOut.Analog[17] = IntegralAccNick / 26;
+DebugOut.Analog[18] = IntegralAccRoll / 26;
+DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
+DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
+DebugOut.Analog[21] = MittelIntegralNick / 26;
+DebugOut.Analog[22] = MittelIntegralRoll / 26;
+DebugOut.Analog[28] = ausgleichNick;
+DebugOut.Analog[29] = ausgleichRoll;
+#define FEHLER_LIMIT  (ABGLEICH_ANZAHL/2)
+#define FEHLER_LIMIT2  4096
+// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
+        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
+        if(IntegralFehlerNick >  FEHLER_LIMIT2)
+         {
+           if(last_n_p)
+           {
+            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
+            ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
+            if(ausgleichNick > 10000) ausgleichNick = 10000;
+            Mess_IntegralNick += ausgleichNick;
+           }
+           else last_n_p = 1;
+         } else  last_n_p = 0;
+        if(IntegralFehlerNick < -FEHLER_LIMIT2)
+         {
+           if(last_n_n)
+            {
+             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
+             ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
+             if(ausgleichNick < -10000) ausgleichNick = -10000;
+             Mess_IntegralNick += ausgleichNick;
+            }
+           else last_n_n = 1;
+         } else  last_n_n = 0;
+        if(cnt > 2) cnt = 2;
+        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
+        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
+//        if(IntegralFehlerNick >  FEHLER_LIMIT)  { AdNeutralNick += (cnt + last_n_p/20);  if(last_n_p++ == 4) {last_n_p = 4;}} else last_n_p = 0;
+//        if(IntegralFehlerNick < -FEHLER_LIMIT)  { AdNeutralNick -= (cnt + last_n_n/20);  if(last_n_n++ == 4) {last_n_n = 4;}} else last_n_n = 0;
+// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
+        cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
+        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
+         {
+           if(last_r_p)
+           {
+            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
+            ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
+            if(ausgleichRoll > 10000) ausgleichRoll = 10000;
+            Mess_IntegralRoll += ausgleichRoll;
+           }
+           else last_r_p = 1;
+         } else  last_r_p = 0;
+        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
+         {
+           if(last_r_n)
+           {
+            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
+            ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
+            if(ausgleichRoll < -10000) ausgleichRoll = -10000;
+            Mess_IntegralRoll += ausgleichRoll;
+           }
+           else last_r_n = 1;
+         } else  last_r_n = 0;
+        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
+        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
+        if(cnt > 2) cnt = 2;
+//        if(IntegralFehlerRoll >  FEHLER_LIMIT)  { AdNeutralRoll += (cnt + last_r_p/20);  if(last_r_p++ == 4) {beeptime =500;last_r_p = 4;}} else last_r_p = 0;
+//        if(IntegralFehlerRoll < -FEHLER_LIMIT)  { AdNeutralRoll -= (cnt + last_r_n/20);  if(last_r_n++ == 4) {beeptime = 60;last_r_n = 4;}} else last_r_n = 0;
+//        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)
+//        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;   // macht nur mit Referenz (Kompass Sinn)
+        cnt = 2;
+//DebugOut.Analog[24] = 10*(AdNeutralRoll - 550);
+  }
+    IntegralAccNick = 0;
+    IntegralAccRoll = 0;
+    IntegralAccZ = 0;
- ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
+    MittelIntegralNick = 0;
+    MittelIntegralRoll = 0;
-    Mess_IntegralNick -= tmp_long;
+    MittelIntegralNick2 = 0;
-    Mess_IntegralRoll -= tmp_long2;
+    MittelIntegralRoll2 = 0;
+    ZaehlMessungen = 0;
+ }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(abs(StickGier) > 20) // war 35
+     {
       if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
+     }
-    tmp_int  = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo  y = ax + bx²
+    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;
     Mess_Integral_Gier -= tmp_int;
-    if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000;  // begrenzen
+    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
-    if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000;
+    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
- ANALOG_ON;     // ADC einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+     {
+        }
        w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
        if(w > 0)
+        {
-          ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
           if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
-          ANALOG_ON;  // ADC einschalten
           if(SignalSchlecht) SignalSchlecht--;
+        }
         else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
+     }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!TimerWerteausgabe--)
+   {
-    TimerWerteausgabe = 49;
+    TimerWerteausgabe = 24;
     DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
     DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
     DebugOut.Analog[2] = Mittelwert_AccNick;
     DebugOut.Analog[3] = Mittelwert_AccRoll;
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[5] = HoehenWert;
-    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
+    DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
     DebugOut.Analog[8] = KompassValue;
     DebugOut.Analog[9] = UBat;
     DebugOut.Analog[10] = SenderOkay;
+    DebugOut.Analog[16] = Mittelwert_AccHoch;
 /*    DebugOut.Analog[16] = motor_rx[0];
     DebugOut.Analog[17] = motor_rx[1];
     DebugOut.Analog[18] = motor_rx[2];
     if(GasMischanteil < 20) GierMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
     SummeNick += DiffNick;                                   // I-Anteil
     if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
     if(SummeNick >  16000) SummeNick =  16000;

Subversion Repositories FlightCtrl

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