Software & Programacion de Mikrokopter

Tema en 'Mikrokopter' iniciado por Pedrosuna, 25 Ago 2009.

  1. Pedrosuna Nuevo Miembro

    Pedrosuna
    Registrado:
    18 Dic 2008
    Mensajes:
    21
    Me Gusta recibidos:
    0
    Hola a todos,

    He abierto este hilo para que todos aportemos lo que sabemos acerca de la programacion del micro de Mikrokopter (ATMEGA644).

    El codigo es bastante engorroso, y sobre todo para los novatos como yo, aunque creo que seria una buena idea conocer qué hace el codigo, para poder incluir algo de nuestra cosecha. Es decir, poder modificar a nuestro antojo (dentro de unos limites) el codigo, incluyendo miniacciones del MK, usando el sensor de altura, etc...

    La finalidad del tema es poder distinguir las distintas variables que componen el codigo, para que se emplean los filtros, saber identificar los lazos de control.....

    Saludos a todos, espero vuestras aportaciones que seguro que son muchas
     
    #1 Pedrosuna, 25 Ago 2009
    Última edición por un moderador: 5 Dic 2011
  2. Pumuky Gurú FPV

    Pumuky
    Registrado:
    16 Sep 2008
    Mensajes:
    4.092
    Me Gusta recibidos:
    0
    Me parece que pìdes demasiado. Creo que la complejidad de ese codigo se escapa a la gran mayoria, bastante que se consiguen hacer algunos ajustes personalizados y desde el MK tool, profundizar en el codigo creo que es solo para usuarios demasiado avanzados.
     
  3. Pedrosuna Nuevo Miembro

    Pedrosuna
    Registrado:
    18 Dic 2008
    Mensajes:
    21
    Me Gusta recibidos:
    0
    Pues esperaremos a esos usuarios demasiado avanzados ! seguro que los habrá ! de todas formas no pretendo que conozcamos el codigo al pie de la letra, sino poder dividirlo en partes, saber donde estan los lazos de control, los filtros usados, etc... Aunque es verdad que parece muy engorroso !! de todos modos gracias pumuky!
     
  4. albertocvr Miembro Activo

    albertocvr
    Registrado:
    9 Abr 2007
    Mensajes:
    1.197
    Me Gusta recibidos:
    0
    Pues aquí lo tienes ...

    /*#######################################################################################
    Flight Control
    #######################################################################################*/
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // + Copyright (c) 04.2007 Holger Buss
    // + Nur f�r den privaten Gebrauch
    // + www.MikroKopter.com
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
    // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist.
    // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
    // + bzgl. der Nutzungsbedingungen aufzunehmen.
    // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
    // + Verkauf von Luftbildaufnahmen, usw.
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
    // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
    // + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
    // + eindeutig als Ursprung verlinkt werden
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
    // + Benutzung auf eigene Gefahr
    // + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
    // + mit unserer Zustimmung zul�ssig
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // + Redistributions of source code (with or without modifications) must retain the above copyright notice,
    // + this list of conditions and the following disclaimer.
    // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
    // + from this software without specific prior written permission.
    // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
    // + for non-commercial use (directly or indirectly)
    // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
    // + with our written permission
    // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
    // + clearly linked as origin
    // + * porting to systems other than hardware from www.mikrokopter.de is not allowed
    // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
    // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
    // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
    // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
    // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
    // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
    // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
    // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
    // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    // + POSSIBILITY OF SUCH DAMAGE.
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    #include "main.h"
    #include "eeprom.c"

    unsigned char h,m,s;
    volatile unsigned int I2CTimeout = 100;
    int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
    int TrimNick, TrimRoll;
    int AdNeutralGierBias;
    int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
    int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
    int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
    volatile float NeutralAccZ = 0;
    unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
    long IntegralNick = 0,IntegralNick2 = 0;
    long IntegralRoll = 0,IntegralRoll2 = 0;
    long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
    long Integral_Gier = 0;
    long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
    long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
    long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
    long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
    volatile long Mess_Integral_Hoch = 0;
    int KompassValue = 0;
    int KompassStartwert = 0;
    int KompassRichtung = 0;
    unsigned int KompassSignalSchlecht = 500;
    unsigned char MAX_GAS,MIN_GAS;
    unsigned char Notlandung = 0;
    unsigned char HoehenReglerAktiv = 0;
    unsigned char TrichterFlug = 0;
    long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
    long ErsatzKompass;
    int ErsatzKompassInGrad; // Kompasswert in Grad
    int GierGyroFehler = 0;
    char GyroFaktor,GyroFaktorGier;
    char IntegralFaktor,IntegralFaktorGier;
    int DiffNick,DiffRoll;
    int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
    volatile unsigned char SenderOkay = 0;
    int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
    char MotorenEin = 0;
    int HoehenWert = 0;
    int SollHoehe = 0;
    int LageKorrekturRoll = 0,LageKorrekturNick = 0;
    //float Ki = FAKTOR_I;
    int Ki = 10300 / 33;
    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_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
    unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250
    unsigned char Parameter_Gyro_D = 8; // Wert : 0-250
    unsigned char Parameter_Gyro_P = 150; // Wert : 10-250
    unsigned char Parameter_Gyro_I = 150; // Wert : 0-250
    unsigned char Parameter_Gier_P = 2; // Wert : 1-20
    unsigned char Parameter_I_Faktor = 10; // Wert : 1-20
    unsigned char Parameter_UserParam1 = 0;
    unsigned char Parameter_UserParam2 = 0;
    unsigned char Parameter_UserParam3 = 0;
    unsigned char Parameter_UserParam4 = 0;
    unsigned char Parameter_UserParam5 = 0;
    unsigned char Parameter_UserParam6 = 0;
    unsigned char Parameter_UserParam7 = 0;
    unsigned char Parameter_UserParam8 = 0;
    unsigned char Parameter_ServoNickControl = 100;
    unsigned char Parameter_ServoRollControl = 100;
    unsigned char Parameter_LoopGasLimit = 70;
    unsigned char Parameter_AchsKopplung1 = 90;
    unsigned char Parameter_AchsKopplung2 = 65;
    unsigned char Parameter_CouplingYawCorrection = 64;
    //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_NaviOperatingRadius;
    unsigned char Parameter_NaviWindCorrection;
    unsigned char Parameter_NaviSpeedCompensation;
    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;
    volatile unsigned char MikroKopterFlags = 0;
    long GIER_GRAD_FAKTOR = 1291;
    signed int KopplungsteilNickRoll,KopplungsteilRollNick;
    unsigned char RequiredMotors = 4;
    unsigned char Motor[MAX_MOTORS];
    signed int tmp_motorwert[MAX_MOTORS];

    int MotorSmoothing(int neu, int alt)
    {
    int motor;
    if(neu > alt) motor = (1*(int)alt + neu) / 2;
    else motor = neu - (alt - neu)*1;
    //if(Poti2 < 20) return(neu);
    return(motor);
    }

    void Piep(unsigned char Anzahl, unsigned int dauer)
    {
    if(MotorenEin) return; //auf keinen Fall im Flug!
    while(Anzahl--)
    {
    beeptime = dauer;
    while(beeptime);
    Delay_ms(dauer * 2);
    }
    }

    //############################################################################
    // Nullwerte ermitteln
    void SetNeutral(void)
    //############################################################################
    {
    unsigned char i;
    unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
    ServoActive = 0; HEF4017R_ON;
    NeutralAccX = 0;
    NeutralAccY = 0;
    NeutralAccZ = 0;
    AdNeutralNick = 0;
    AdNeutralRoll = 0;
    AdNeutralGier = 0;
    AdNeutralGierBias = 0;
    Parameter_AchsKopplung1 = 0;
    Parameter_AchsKopplung2 = 0;
    ExpandBaro = 0;
    CalibrierMittelwert();
    Delay_ms_Mess(100);
    CalibrierMittelwert();
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung aktiviert?
    {
    if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
    }
    #define NEUTRAL_FILTER 32
    for(i=0; i<NEUTRAL_FILTER; i++)
    {
    Delay_ms_Mess(10);
    gier_neutral += AdWertGier;
    nick_neutral += AdWertNick;
    roll_neutral += AdWertRoll;
    }
    AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
    AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
    AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);
    AdNeutralGierBias = AdNeutralGier;
    StartNeutralRoll = AdNeutralRoll;
    StartNeutralNick = AdNeutralNick;
    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
    {
    NeutralAccY = abs(Mittelwert_AccRoll) / (2*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]);
    NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
    NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
    }

    MesswertNick = 0;
    MesswertRoll = 0;
    MesswertGier = 0;
    Delay_ms_Mess(100);
    Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
    Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
    IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
    IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
    Mess_IntegralNick2 = IntegralNick;
    Mess_IntegralRoll2 = IntegralRoll;
    Mess_Integral_Gier = 0;
    StartLuftdruck = Luftdruck;
    HoeheD = 0;
    Mess_Integral_Hoch = 0;
    KompassStartwert = KompassValue;
    GPS_Neutral();
    beeptime = 50;
    Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
    Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
    ExternHoehenValue = 0;
    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
    GierGyroFehler = 0;
    SendVersionToNavi = 1;
    LED_Init();
    MikroKopterFlags |= FLAG_CALIBRATE;
    FromNaviCtrl_Value.Kalman_K = -1;
    FromNaviCtrl_Value.Kalman_MaxDrift = 0;
    FromNaviCtrl_Value.Kalman_MaxFusion = 32;
    Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;
    Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;
    Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;
    Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;
    // ServoActive = 1;
    SenderOkay = 100;
    }

    //############################################################################
    // Bearbeitet die Messwerte
    void Mittelwert(void)
    //############################################################################
    {
    static signed long tmpl,tmpl2,tmpl3,tmpl4;
    static signed int oldNick, oldRoll, d2Roll, d2Nick;
    signed long winkel_nick, winkel_roll;

    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
    // MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
    MesswertNick = (signed int) AdWertNickFilter / 8;
    MesswertRoll = (signed int) AdWertRollFilter / 8;
    RohMesswertNick = MesswertNick;
    RohMesswertRoll = MesswertRoll;
    //DebugOut.Analog[21] = MesswertNick;
    //DebugOut.Analog[22] = MesswertRoll;
    //DebugOut.Analog[22] = Mess_Integral_Gier;
    //DebugOut.Analog[21] = MesswertNick;
    //DebugOut.Analog[22] = MesswertRoll;

    // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++
    Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L;
    Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L;
    Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 3 + ((long)AdWertAccHoch)) / 4L;
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
    NaviAccNick += AdWertAccNick;
    NaviAccRoll += AdWertAccRoll;
    NaviCntAcc++;
    IntegralAccZ += Aktuell_az - NeutralAccZ;

    //++++++++++++++++++++++++++++++++++++++++++++++++
    // ADC einschalten
    ANALOG_ON;
    AdReady = 0;
    //++++++++++++++++++++++++++++++++++++++++++++++++

    if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;
    else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;
    else winkel_roll = Mess_IntegralRoll;

    if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;
    else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;
    else winkel_nick = Mess_IntegralNick;

    // Gier ++++++++++++++++++++++++++++++++++++++++++++++++
    Mess_Integral_Gier += MesswertGier;
    ErsatzKompass += MesswertGier;
    // Kopplungsanteil +++++++++++++++++++++++++++++++++++++
    if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
    {
    tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
    tmpl3 *= Parameter_AchsKopplung2; //65
    tmpl3 /= 4096L;
    tmpl4 = (MesswertNick * winkel_roll) / 2048L;
    tmpl4 *= Parameter_AchsKopplung2; //65
    tmpl4 /= 4096L;
    KopplungsteilNickRoll = tmpl3;
    KopplungsteilRollNick = tmpl4;
    tmpl4 -= tmpl3;
    ErsatzKompass += tmpl4;
    if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen

    tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
    tmpl *= Parameter_AchsKopplung1; // 90
    tmpl /= 4096L;
    tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
    tmpl2 *= Parameter_AchsKopplung1;
    tmpl2 /= 4096L;
    if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
    //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
    }
    else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;

    TrimRoll = tmpl - tmpl2 / 100L;
    TrimNick = -tmpl2 + tmpl / 100L;

    // Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++
    if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360� Umschlag
    if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
    // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
    Mess_IntegralRoll2 += MesswertRoll + TrimRoll;
    Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll;
    if(Mess_IntegralRoll > Umschlag180Roll)
    {
    Mess_IntegralRoll = -(Umschlag180Roll - 25000L);
    Mess_IntegralRoll2 = Mess_IntegralRoll;
    }
    if(Mess_IntegralRoll <-Umschlag180Roll)
    {
    Mess_IntegralRoll = (Umschlag180Roll - 25000L);
    Mess_IntegralRoll2 = Mess_IntegralRoll;
    }
    // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
    Mess_IntegralNick2 += MesswertNick + TrimNick;
    Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick;
    if(Mess_IntegralNick > Umschlag180Nick)
    {
    Mess_IntegralNick = -(Umschlag180Nick - 25000L);
    Mess_IntegralNick2 = Mess_IntegralNick;
    }
    if(Mess_IntegralNick <-Umschlag180Nick)
    {
    Mess_IntegralNick = (Umschlag180Nick - 25000L);
    Mess_IntegralNick2 = Mess_IntegralNick;
    }

    Integral_Gier = Mess_Integral_Gier;
    IntegralNick = Mess_IntegralNick;
    IntegralRoll = Mess_IntegralRoll;
    IntegralNick2 = Mess_IntegralNick2;
    IntegralRoll2 = Mess_IntegralRoll2;

    #define D_LIMIT 128

    MesswertNick = HiResNick / 8;
    MesswertRoll = HiResRoll / 8;

    if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000;
    if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; }
    else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; }
    if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000;
    if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; }
    else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; }

    if(Parameter_Gyro_D)
    {
    d2Nick = HiResNick - oldNick;
    oldNick = (oldNick + HiResNick)/2;
    if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
    else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
    MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;
    d2Roll = HiResRoll - oldRoll;
    oldRoll = (oldRoll + HiResRoll)/2;
    if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
    else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
    MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
    HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);
    HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);
    }

    if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
    else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
    if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
    else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;

    if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
    {
    if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256);
    else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);
    if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256);
    else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256);
    }

    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;
    }

    //############################################################################
    // Messwerte beim Ermitteln der Nullage
    void CalibrierMittelwert(void)
    //############################################################################
    {
    if(PlatinenVersion == 13) SucheGyroOffset();
    // ADC auschalten, damit die Werte sich nicht w�hrend der Berechnung �ndern
    ANALOG_OFF;
    MesswertNick = AdWertNick;
    MesswertRoll = AdWertRoll;
    MesswertGier = AdWertGier;
    Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
    Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
    Mittelwert_AccHoch = (long)AdWertAccHoch;
    // 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.WinkelUmschlagRoll * 2500L;
    }

    //############################################################################
    // Senden der Motorwerte per I2C-Bus
    void SendMotorData(void)
    //############################################################################
    {
    unsigned char i;
    if(!MotorenEin)
    {
    MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
    for(i=0;i<MAX_MOTORS;i++)
    {
    if(!PC_MotortestActive) MotorTest = 0;
    Motor = MotorTest;
    }
    if(PC_MotortestActive) PC_MotortestActive--;
    }
    else MikroKopterFlags |= FLAG_MOTOR_RUN;

    DebugOut.Analog[12] = Motor[0];
    DebugOut.Analog[13] = Motor[1];
    DebugOut.Analog[14] = Motor[3];
    DebugOut.Analog[15] = Motor[2];

    //Start I2C Interrupt Mode
    twi_state = 0;
    motor = 0;
    i2c_start();
    }



    //############################################################################
    // 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; }
    CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
    CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,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_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
    CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
    CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,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_UserParam3,EE_Parameter.UserParam3,0,255);
    CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
    CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
    CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
    CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
    CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
    CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
    CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl,0,255);
    CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
    CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255);
    CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2,0,255);
    CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,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_ExternalControl,EE_Parameter.ExternalControl,0,255);
    Ki = 10300 / (Parameter_I_Faktor + 1);
    MAX_GAS = EE_Parameter.Gas_Max;
    MIN_GAS = EE_Parameter.Gas_Min;
    }

    void start_engines(void)
    {

    }

    //############################################################################
    //
    void MotorRegler(void)
    //############################################################################
    {
    int 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;
    static unsigned int RcLostTimer;
    static unsigned char delay_neutral = 0;
    static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
    static int hoehenregler = 0;
    static char TimerWerteausgabe = 0;
    static char NeueKompassRichtungMerken = 0;
    static long ausgleichNick, ausgleichRoll;
    int IntegralNickMalFaktor,IntegralRollMalFaktor;
    unsigned char i;
    Mittelwert();

    if (ExternEvent.key!=0)
    {
    switch((int)ExternEvent.key)
    {
    case 1:
    // start engines
    modell_fliegt = 1;
    MotorenEin = 1;
    sollGier = 0;
    Mess_Integral_Gier = 0;
    Mess_Integral_Gier2 = 0;
    Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
    Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
    Mess_IntegralNick2 = IntegralNick;
    Mess_IntegralRoll2 = IntegralRoll;
    SummeNick = 0;
    SummeRoll = 0;

    break;

    case 2:
    // stop engines
    MotorenEin = 0;
    delay_ausschalten = 200;
    modell_fliegt = 0;

    break;

    }


    ExternEvent.key=0;
    }
    GRN_ON;
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Gaswert ermitteln
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    GasMischanteil = StickGas;
    if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Empfang schlecht
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
    {
    if(!PcZugriff)
    {
    if(BeepMuster == 0xffff)
    {
    beeptime = 15000;
    BeepMuster = 0x0c00;
    }
    }
    if(RcLostTimer) RcLostTimer--;
    else
    {
    MotorenEin = 0;
    Notlandung = 0;
    }
    ROT_ON;
    if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken
    {
    GasMischanteil = EE_Parameter.NotGas;
    Notlandung = 1;
    PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
    PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
    PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
    PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
    PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
    }
    else MotorenEin = 0;
    }
    else
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Emfang gut
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay > 140)
    {
    Notlandung = 0;
    RcLostTimer = EE_Parameter.NotGasZeit * 50;
    if(GasMischanteil > 40 && MotorenEin)
    {
    if(modell_fliegt < 0xffff) modell_fliegt++;
    }
    if((modell_fliegt < 256))
    {
    SummeNick = 0;
    SummeRoll = 0;
    if(modell_fliegt == 250)
    {
    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
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
    {
    if(++delay_neutral > 200) // nicht sofort
    {
    GRN_OFF;
    MotorenEin = 0;
    delay_neutral = 0;
    modell_fliegt = 0;
    if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
    {
    unsigned char setting=1;
    if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
    if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
    if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
    if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
    if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
    SetActiveParamSetNumber(setting); // aktiven Datensatz merken
    }
    // else
    if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
    {
    WinkelOut.CalcState = 1;
    beeptime = 1000;
    }
    else
    {
    ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung aktiviert?
    {
    if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
    }
    SetNeutral();
    ServoActive = 1;
    DDRD |=0x80; // enable J7 -> Servo signal
    Piep(GetActiveParamSetNumber(),120);
    }
    }
    }
    else
    if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern
    {
    if(++delay_neutral > 200) // nicht sofort
    {
    GRN_OFF;
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte l�schen
    MotorenEin = 0;
    delay_neutral = 0;
    modell_fliegt = 0;
    SetNeutral();
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
    Piep(GetActiveParamSetNumber(),120);
    }
    }
    else delay_neutral = 0;
    }
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Gas ist unten
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
    {
    // Starten
    if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
    {
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Einschalten
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(++delay_einschalten > 200)
    {
    delay_einschalten = 200;
    modell_fliegt = 1;
    MotorenEin = 1;
    sollGier = 0;
    Mess_Integral_Gier = 0;
    Mess_Integral_Gier2 = 0;
    Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
    Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
    Mess_IntegralNick2 = IntegralNick;
    Mess_IntegralRoll2 = IntegralRoll;
    SummeNick = 0;
    SummeRoll = 0;
    MikroKopterFlags |= FLAG_START;
    }
    }
    else delay_einschalten = 0;
    //Auf Neutralwerte setzen
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Auschalten
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
    {
    if(++delay_ausschalten > 200) // nicht sofort
    {
    MotorenEin = 0;
    delay_ausschalten = 200;
    modell_fliegt = 0;
    }
    }
    else delay_ausschalten = 0;
    }
    }

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // neue Werte von der Funke
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    if(!NewPpmData-- || Notlandung)
    {
    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;
    StickNick = stick_nick - (GPS_Nick + GPS_Nick2);

    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
    StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);

    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;
    GyroFaktor = (Parameter_Gyro_P + 10.0);
    IntegralFaktor = Parameter_Gyro_I;
    GyroFaktorGier = (Parameter_Gyro_P + 10.0);
    IntegralFaktorGier = Parameter_Gyro_I;

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //+ Analoge Steuerung per Seriell
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    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;
    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/STICK_GAIN) > MaxStickNick)
    {
    MaxStickNick = abs(StickNick)/STICK_GAIN;
    if(MaxStickNick > 100) MaxStickNick = 100;
    }
    else MaxStickNick--;
    if(abs(StickRoll/STICK_GAIN) > MaxStickRoll)
    {
    MaxStickRoll = abs(StickRoll)/STICK_GAIN;
    if(MaxStickRoll > 100) MaxStickRoll = 100;
    }
    else MaxStickRoll--;
    if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Looping?
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_LINKS) Looping_Links = 1;
    else
    {
    {
    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.BitConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
    else
    {
    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.BitConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
    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.BitConfig & 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

    if(Looping_Roll || Looping_Nick)
    {
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
    TrichterFlug = 1;
    }


    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Bei Empfangsausfall im Flug
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(Notlandung)
    {
    StickGier = 0;
    StickNick = 0;
    StickRoll = 0;
    GyroFaktor = 90;
    IntegralFaktor = 120;
    GyroFaktorGier = 90;
    IntegralFaktorGier = 120;
    Looping_Roll = 0;
    Looping_Nick = 0;
    }


    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // 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)
    {
    IntegralAccNick = 0;
    IntegralAccRoll = 0;
    MittelIntegralNick = 0;
    MittelIntegralRoll = 0;
    MittelIntegralNick2 = 0;
    MittelIntegralRoll2 = 0;
    Mess_IntegralNick2 = Mess_IntegralNick;
    Mess_IntegralRoll2 = Mess_IntegralRoll;
    ZaehlMessungen = 0;
    LageKorrekturNick = 0;
    LageKorrekturRoll = 0;
    }

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin))
    {
    long tmp_long, tmp_long2;
    if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/)
    {
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
    tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
    tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
    if((MaxStickNick > 64) || (MaxStickRoll > 64))
    {
    tmp_long /= 2;
    tmp_long2 /= 2;
    }
    if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
    {
    tmp_long /= 3;
    tmp_long2 /= 3;
    }
    if(tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
    if(tmp_long < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
    if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
    if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
    }
    else
    {
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
    tmp_long /= 16;
    tmp_long2 /= 16;
    if((MaxStickNick > 64) || (MaxStickRoll > 64))
    {
    tmp_long /= 3;
    tmp_long2 /= 3;
    }
    if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
    {
    tmp_long /= 3;
    tmp_long2 /= 3;
    }

    #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(Poti2 > 20) { tmp_long = 0; tmp_long2 = 0;}
    Mess_IntegralNick -= tmp_long;
    Mess_IntegralRoll -= tmp_long2;
    }
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(ZaehlMessungen >= ABGLEICH_ANZAHL)
    {
    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 && !TrichterFlug && EE_Parameter.Driftkomp)
    {
    MittelIntegralNick /= ABGLEICH_ANZAHL;
    MittelIntegralRoll /= ABGLEICH_ANZAHL;
    IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
    IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
    IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL;
    #define MAX_I 0//(Poti2/10)
    // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
    // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;

    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;

    if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1))
    {
    LageKorrekturNick /= 2;
    LageKorrekturRoll /= 2;
    }

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Gyro-Drift ermitteln
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
    tmp_long = IntegralNick2 - IntegralNick;
    tmp_long2 = IntegralRoll2 - IntegralRoll;
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;

    IntegralFehlerNick = tmp_long;
    IntegralFehlerRoll = tmp_long2;
    Mess_IntegralNick2 -= IntegralFehlerNick;
    Mess_IntegralRoll2 -= IntegralFehlerRoll;

    // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
    // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
    if(EE_Parameter.Driftkomp)
    {
    if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
    }
    //DebugOut.Analog[22] = MittelIntegralRoll / 26;
    //DebugOut.Analog[24] = GierGyroFehler;
    GierGyroFehler = 0;


    /*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;
    //MittelIntegralRoll = MittelIntegralRoll;
    //DebugOut.Analog[28] = ausgleichNick;
    /*
    DebugOut.Analog[29] = ausgleichRoll;
    DebugOut.Analog[30] = LageKorrekturRoll * 10;
    */

    #define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2)
    #define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4
    #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16
    #define BEWEGUNGS_LIMIT 20000
    // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
    cnt = 1;// + labs(IntegralFehlerNick) / 4096;
    if(labs(IntegralFehlerNick) > FEHLER_LIMIT1) cnt = 4;
    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
    {
    if(IntegralFehlerNick > FEHLER_LIMIT2)
    {
    if(last_n_p)
    {
    cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
    ausgleichNick = IntegralFehlerNick / 8;
    if(ausgleichNick > 5000) ausgleichNick = 5000;
    LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
    }
    else last_n_p = 1;
    } else last_n_p = 0;
    if(IntegralFehlerNick < -FEHLER_LIMIT2)
    {
    if(last_n_n)
    {
    cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
    ausgleichNick = IntegralFehlerNick / 8;
    if(ausgleichNick < -5000) ausgleichNick = -5000;
    LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
    }
    else last_n_n = 1;
    } else last_n_n = 0;
    }
    else
    {
    cnt = 0;
    KompassSignalSchlecht = 1000;
    }
    if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
    if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
    if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
    if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;

    // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
    cnt = 1;// + labs(IntegralFehlerNick) / 4096;
    if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4;
    ausgleichRoll = 0;
    if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
    {
    if(IntegralFehlerRoll > FEHLER_LIMIT2)
    {
    if(last_r_p)
    {
    cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
    ausgleichRoll = IntegralFehlerRoll / 8;
    if(ausgleichRoll > 5000) ausgleichRoll = 5000;
    LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
    }
    else last_r_p = 1;
    } else last_r_p = 0;
    if(IntegralFehlerRoll < -FEHLER_LIMIT2)
    {
    if(last_r_n)
    {
    cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
    ausgleichRoll = IntegralFehlerRoll / 8;
    if(ausgleichRoll < -5000) ausgleichRoll = -5000;
    LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
    }
    else last_r_n = 1;
    } else last_r_n = 0;
    } else
    {
    cnt = 0;
    KompassSignalSchlecht = 1000;
    }
    if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
    if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
    if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
    if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
    }
    else
    {
    LageKorrekturRoll = 0;
    LageKorrekturNick = 0;
    TrichterFlug = 0;
    }

    if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++
    MittelIntegralNick_Alt = MittelIntegralNick;
    MittelIntegralRoll_Alt = MittelIntegralRoll;
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++
    IntegralAccNick = 0;
    IntegralAccRoll = 0;
    IntegralAccZ = 0;
    MittelIntegralNick = 0;
    MittelIntegralRoll = 0;
    MittelIntegralNick2 = 0;
    MittelIntegralRoll2 = 0;
    ZaehlMessungen = 0;
    } // ZaehlMessungen >= ABGLEICH_ANZAHL


    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // 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;
    };
    }
    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 > 50000) Mess_Integral_Gier = 50000; // begrenzen
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Kompass
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //DebugOut.Analog[16] = KompassSignalSchlecht;

    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
    {
    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;
    if(abs(MesswertGier) > 128)
    {
    fehler = 0;
    }
    if(!KompassSignalSchlecht && w < 25)
    {
    GierGyroFehler += fehler;
    if(NeueKompassRichtungMerken)
    {
    beeptime = 200;
    // KompassStartwert = KompassValue;
    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
    KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
    NeueKompassRichtungMerken = 0;
    }
    }
    ErsatzKompass += (fehler * 8) / korrektur;
    w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren
    w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
    if(w >= 0)
    {
    if(!KompassSignalSchlecht)
    {
    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; // Begrenzen
    else
    if(v < -w) v = -w;
    Mess_Integral_Gier += v;
    }
    if(KompassSignalSchlecht) KompassSignalSchlecht--;
    }
    else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
    }
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Debugwerte zuordnen
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(!TimerWerteausgabe--)
    {
    TimerWerteausgabe = 24;

    DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
    DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
    DebugOut.Analog[2] = Mittelwert_AccNick / 4;
    DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
    DebugOut.Analog[4] = MesswertGier;
    DebugOut.Analog[5] = HoehenWert;
    DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;
    DebugOut.Analog[8] = KompassValue;
    DebugOut.Analog[9] = UBat;
    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
    DebugOut.Analog[10] = SenderOkay;


    DebugOut.Analog[16]=MotorenEin;
    //DebugOut.Analog[16] = Mittelwert_AccHoch;
    //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
    //DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
    DebugOut.Analog[19] = WinkelOut.CalcState;
    DebugOut.Analog[20] = ServoValue;
    // DebugOut.Analog[24] = MesswertNick/2;
    // DebugOut.Analog[25] = MesswertRoll/2;
    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
    // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
    // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
    DebugOut.Analog[30] = GPS_Nick;
    DebugOut.Analog[31] = GPS_Roll;


    // DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
    // if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;

    /* DebugOut.Analog[16] = motor_rx[0];
    DebugOut.Analog[17] = motor_rx[1];
    DebugOut.Analog[18] = motor_rx[2];
    DebugOut.Analog[19] = motor_rx[3];
    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
    DebugOut.Analog[20] /= 14;
    DebugOut.Analog[21] = motor_rx[4];
    DebugOut.Analog[22] = motor_rx[5];
    DebugOut.Analog[23] = motor_rx[6];
    DebugOut.Analog[24] = motor_rx[7];
    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
    */
    // DebugOut.Analog[9] = MesswertNick;
    // DebugOut.Analog[9] = SollHoehe;
    // DebugOut.Analog[10] = Mess_Integral_Gier / 128;
    // DebugOut.Analog[11] = KompassStartwert;
    // DebugOut.Analog[10] = Parameter_Gyro_I;
    // DebugOut.Analog[10] = EE_Parameter.Gyro_I;
    // DebugOut.Analog[9] = KompassRichtung;
    // DebugOut.Analog[10] = GasMischanteil;
    // DebugOut.Analog[3] = HoeheD * 32;
    // DebugOut.Analog[4] = hoehenregler;
    }

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;};

    if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;
    if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0;

    #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;

    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));

    // Maximalwerte abfangen
    // #define MAX_SENSOR (4096*STICK_GAIN)
    #define MAX_SENSOR (4096*4)
    if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
    if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
    if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // all BL-Ctrl connected?
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(MissingMotor) if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
    {
    modell_fliegt = 1;
    GasMischanteil = MIN_GAS;
    }

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // H�henregelung
    // Die H�henregelung schw�cht lediglich das Gas ab, erh�ht es allerdings nicht
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    GasMischanteil *= STICK_GAIN;
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H�henregelung
    {
    int tmp_int;
    static char delay = 100;
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird �ber Schalter gesteuert
    {
    if(Parameter_MaxHoehe < 50)
    {
    if(!delay--)
    {
    if((MessLuftdruck > 1000) && OCR0A < 254)
    {
    if(OCR0A < 244)
    {
    ExpandBaro -= 10;
    OCR0A = DruckOffsetSetting - ExpandBaro;
    }
    else OCR0A = 254;
    beeptime = 300;
    delay = 250;
    }
    else
    if((MessLuftdruck < 100) && OCR0A > 1)
    {
    if(OCR0A > 10)
    {
    ExpandBaro += 10;
    OCR0A = DruckOffsetSetting - ExpandBaro;
    }
    else OCR0A = 1;
    beeptime = 300;
    delay = 250;
    }
    else
    {
    SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
    HoehenReglerAktiv = 0;
    delay = 1;
    }
    }
    }
    else
    {
    HoehenReglerAktiv = 1;
    delay = 200;
    }
    }
    else
    {
    SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
    HoehenReglerAktiv = 1;
    }

    if(Notlandung) SollHoehe = 0;
    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)/(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;
    hoehenregler = (hoehenregler*15 + h) / 16;
    if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
    {
    if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
    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) * 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 * 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_nick = 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_nick > tmp_int) pd_ergebnis_nick = tmp_int;
    if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // 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_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;

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Universal Mixer
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    for(i=0; i<MAX_MOTORS; i++)
    {
    signed int tmp_int;
    if(Mixer.Motor[0] > 0)
    {
    tmp_int = ((long)GasMischanteil * Mixer.Motor[0]) / 64L;
    tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[1]) / 64L;
    tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[2]) / 64L;
    tmp_int += ((long)GierMischanteil * Mixer.Motor[3]) / 64L;
    tmp_motorwert = MotorSmoothing(tmp_int,tmp_motorwert); // Filter
    tmp_int = tmp_motorwert / STICK_GAIN;
    CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS);
    Motor = tmp_int;
    }
    else Motor = 0;
    }
    /*
    if(Poti1 > 20) Motor1 = 0;
    if(Poti1 > 90) Motor6 = 0;
    if(Poti1 > 140) Motor2 = 0;
    //if(Poti1 > 200) Motor7 = 0;
    */
    }
     
  5. Pedrosuna Nuevo Miembro

    Pedrosuna
    Registrado:
    18 Dic 2008
    Mensajes:
    21
    Me Gusta recibidos:
    0
    Una pregunta albertocvr, ¿Has compilado el codigo con AVR Studio? Es decir, ¿Has obtenido el archivo.hex a partir de los archivos .c .h etc.. con dicho programa? es para hacer alguna pregunta. Gracias
     

Compartir esta página