Subversion Repositories FlightCtrl

/*#######################################################################################

Flight Control

#######################################################################################*/

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Copyright (c) Holger Buss, Ingo Busker

// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY

// + 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 oder Nutzung 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 the sources to other systems or using the software on other systems (except 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 "mymath.h"

#include "isqrt.h"

unsigned char h,m,s;

unsigned int BaroExpandActive = 0;

int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;

int TrimNick, TrimRoll;

int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;

int Mittelwert_AccNick, Mittelwert_AccRoll;

unsigned int NeutralAccX=0, NeutralAccY=0;

int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;

int NeutralAccZ = 0;

unsigned char ControlHeading = 0;// in 2°

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;

long SummeNick=0,SummeRoll=0;

volatile long Mess_Integral_Hoch = 0;

int  KompassValue = -1;

int  KompassSollWert = 0;

int  KompassRichtung = 0;

char CalculateCompassTimer = 100;

unsigned char KompassFusion = 32;

unsigned int  KompassSignalSchlecht = 50;

unsigned char  MAX_GAS,MIN_GAS;

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 StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023;

//int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;

unsigned char Poti[9] = {0,0,0,0,0,0,0,0};

volatile unsigned char SenderOkay = 0;

int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;

char MotorenEin = 0,StartTrigger = 0;

long HoehenWert = 0;

long SollHoehe = 0;

long FromNC_AltitudeSetpoint = 0;

unsigned char FromNC_AltitudeSpeed = 0;

unsigned char carefree_old = 50; // to make the Beep when switching

int CompassGierSetpoint = 0;

int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 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_HoehenSchalter     = 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_Hoehe_GPS_Z = 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_Gyro_Gier_P = 150;      // Wert : 10-250

unsigned char Parameter_Gyro_Gier_I = 150;      // Wert : 10-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;

unsigned char Parameter_GlobalConfig;

unsigned char Parameter_ExtraConfig;

unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;

unsigned char CareFree = 0;

const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; // 15° steps

signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;

int MaxStickNick = 0,MaxStickRoll = 0;

unsigned int  modell_fliegt = 0;

volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0;

long GIER_GRAD_FAKTOR = 1291;

signed int KopplungsteilNickRoll,KopplungsteilRollNick;

signed int tmp_motorwert[MAX_MOTORS];

char VarioCharacter = ' ';

#define LIMIT_MIN(value, min) {if(value <= min) value = min;}

#define LIMIT_MAX(value, max) {if(value >= max) value = max;}

#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;}

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//  Debugwerte zuordnen

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void CopyDebugValues(void)

{

    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] = (signed int) AdNeutralGier - AdWertGier;

    DebugOut.Analog[5] = HoehenWert/5;

    DebugOut.Analog[6] = AdWertAccHoch;//(Mess_Integral_Hoch / 512);// Aktuell_az;

    DebugOut.Analog[8] = KompassValue;

    DebugOut.Analog[9] = UBat;

    DebugOut.Analog[10] = SenderOkay;

    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;

    DebugOut.Analog[12] = Motor[0].SetPoint;

    DebugOut.Analog[13] = Motor[1].SetPoint;

    DebugOut.Analog[14] = Motor[2].SetPoint;

    DebugOut.Analog[15] = Motor[3].SetPoint;

    DebugOut.Analog[20] = ServoNickValue;

    DebugOut.Analog[22] = Capacity.ActualCurrent;

    DebugOut.Analog[23] = Capacity.UsedCapacity;

        DebugOut.Analog[24] = SollHoehe/5;     

//    DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;

//    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;

    DebugOut.Analog[27] = KompassSollWert;

        DebugOut.Analog[29] = Capacity.MinOfMaxPWM;

    DebugOut.Analog[30] = GPS_Nick;

    DebugOut.Analog[31] = GPS_Roll;

    if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe;

}

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

 }

}

//############################################################################

// Messwerte beim Ermitteln der Nullage

void CalibrierMittelwert(void)

//############################################################################

{

    unsigned char i;

    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 * AdWertAccNick;

        Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll;

   // ADC einschalten

    ANALOG_ON;

   for(i=0;i<8;i++)

    {

     int tmp;

         tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 127;

         LIMIT_MIN_MAX(tmp, 0, 255);

     if(Poti[i] > tmp) Poti[i]--;  else  if(Poti[i] < tmp) Poti[i]++;

        }

        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;

        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;

}

//############################################################################

//  Nullwerte ermitteln

void SetNeutral(unsigned char AccAdjustment)

//############################################################################

{

        unsigned char i;

        unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;

    VersionInfo.HardwareError[0] = 0;

    HEF4017R_ON;

        NeutralAccX = 0;

        NeutralAccY = 0;

        NeutralAccZ = 0;

    AdNeutralNick = 0;

        AdNeutralRoll = 0;

        AdNeutralGier = 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);

     StartNeutralRoll = AdNeutralRoll;

     StartNeutralNick = AdNeutralNick;

     if(AccAdjustment)

     {

            NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);

            NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);

            NeutralAccZ = Aktuell_az;

                // Save ACC neutral settings to eeprom

                SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX);

                SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY);

                SetParamWord(PID_ACC_TOP,  (uint16_t)NeutralAccZ);

    }

    else

    {

                // restore from eeprom

                NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK);

                NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL);

                NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP);

                // strange settings?

                if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048) || ((unsigned int) NeutralAccZ > 1024))

                {

                        printf("\n\rACC not calibrated!\r\n");

                        NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);

                        NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);

                NeutralAccZ = Aktuell_az;

                }

    }

    MesswertNick = 0;

    MesswertRoll = 0;

    MesswertGier = 0;

    Delay_ms_Mess(100);

    Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick;

    Mittelwert_AccRoll = ACC_AMPLIFY * 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;

    VarioMeter = 0;

    Mess_Integral_Hoch = 0;

    KompassSollWert = KompassValue;

        KompassSignalSchlecht = 100;

    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();

    FC_StatusFlags |= FC_STATUS_CALIBRATE;

    FromNaviCtrl_Value.Kalman_K = -1;

    FromNaviCtrl_Value.Kalman_MaxDrift = 0;

    FromNaviCtrl_Value.Kalman_MaxFusion = 32;

   for(i=0;i<8;i++)

    {

     Poti[i] =  PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 127;

        }

    SenderOkay = 100;

    if(ServoActive)

         {

                HEF4017R_ON;

                DDRD  |=0x80; // enable J7 -> Servo signal

     }

        if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; };

        if((AdNeutralRoll < 150 * 16) || (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; };

        if((AdNeutralGier < 150 * 2)  || (AdNeutralGier > 850 * 2))  { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; };

        if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; };

        if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; };

        if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; };

    carefree_old = 70;

}

//############################################################################

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

    MesswertNick = (signed int) AdWertNickFilter / 8;

    MesswertRoll = (signed int) AdWertRollFilter / 8;

    RohMesswertNick = MesswertNick;

    RohMesswertRoll = MesswertRoll;

// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++

        Mittelwert_AccNick = (Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * AdWertAccNick))) / 4L;

        Mittelwert_AccRoll = (Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * AdWertAccRoll))) / 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 && (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;

   d2Roll = HiResRoll - oldRoll;

   oldRoll = (oldRoll + HiResRoll)/2;

   if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;

   else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;

   MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;

   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(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);

  }

}

//############################################################################

// Senden der Motorwerte per I2C-Bus

void SendMotorData(void)

//############################################################################

{

 unsigned char i;

    if(!MotorenEin)

        {

         FC_StatusFlags &= ~(FC_STATUS_MOTOR_RUN | FC_STATUS_FLY);

                 for(i=0;i<MAX_MOTORS;i++)

                  {

                   if(!PC_MotortestActive)  MotorTest[i] = 0;

                   Motor[i].SetPoint = MotorTest[i];

                   Motor[i].SetPointLowerBits = 0;

/*

 Motor[i].SetPoint = MotorTest[i] / 4;            // testing the high resolution

 Motor[i].SetPointLowerBits = MotorTest[i] % 4;

*/

                  }

          if(PC_MotortestActive) PC_MotortestActive--;

        }

        else FC_StatusFlags |= FC_STATUS_MOTOR_RUN;

    if(I2C_TransferActive)

         {

          I2C_TransferActive = 0; // enable for the next time

         }

        else

    {

     motor_write = 0;

     I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode

        }

}

//############################################################################

// Trägt ggf. das Poti als Parameter ein

void ParameterZuordnung(void)

//############################################################################

{

 unsigned char tmp,i;

 #define CHK_POTI(b,a) {if(a < 248) b = a; else b = Poti[255 - a];}

 #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max);}

  for(i=0;i<8;i++)

    {

     int tmp2;

         tmp2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 127;

         if(tmp2 > 255) tmp2 = 255; else if(tmp2 < 0) tmp2 = 0;

     if(tmp2 != Poti[i])

          {

           Poti[i] += (tmp2 - Poti[i]) / 4;

       if(Poti[i] > tmp2) Poti[i]--;

           else Poti[i]++;

          }

        }

 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_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);

 CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);

 CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);

 CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3);

 CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4);

 CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5);

 CHK_POTI(Parameter_HoehenSchalter,EE_Parameter.MaxHoehe);

 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung);

 CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z);

 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung);

 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I);

 CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D);

 CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P);

 CHK_POTI(Parameter_Gyro_Gier_I,EE_Parameter.Gyro_Gier_I);

 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor);

 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1);

 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2);

 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3);

 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4);

 CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5);

 CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6);

 CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7);

 CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8);

 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl);

 CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl);

 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit);

 CHK_POTI(Parameter_AchsKopplung1,EE_Parameter.AchsKopplung1);

 CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2);

 CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection);

 Parameter_GlobalConfig = EE_Parameter.GlobalConfig;

 Parameter_ExtraConfig = EE_Parameter.ExtraConfig;

// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);

 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);

 CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);

 Ki = 10300 / (Parameter_I_Faktor + 1);

 MAX_GAS = EE_Parameter.Gas_Max;

 MIN_GAS = EE_Parameter.Gas_Min;

 tmp = EE_Parameter.OrientationModeControl;

 if(tmp > 50)

   {

#ifdef SWITCH_LEARNS_CAREFREE

//    if(!CareFree)  ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;

#endif

        CareFree = 1;

    if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0;

    if(carefree_old != CareFree)

    {

      if(carefree_old < 3)

           {

                if(CareFree) beeptime = 1500;

            else beeptime = 200;

        carefree_old = CareFree;

           } else carefree_old--;

        }  

        if(FromNaviCtrl.CompassValue < 0 && CareFree) VersionInfo.HardwareError[0] |= FC_ERROR0_CAREFREE; else VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE;

   }

   else

   {

    CareFree = 0;

        carefree_old = 10;

   }   

 if(FromNaviCtrl.CompassValue < 0 && MotorenEin && CareFree && BeepMuster == 0xffff) // ungültiger Kompasswert

        {

         beeptime = 15000;

         BeepMuster = 0xA400;

         CareFree = 0;

    }

 if(CareFree) { FC_StatusFlags2 |= FC_STATUS2_CAREFREE; if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;} else FC_StatusFlags2 &= ~FC_STATUS2_CAREFREE;

}

//############################################################################

//

void MotorRegler(void)

//############################################################################

{

         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;