Subversion Repositories FlightCtrl

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

Flight Control

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

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

// + www.MikroKopter.com

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

// + Software Nutzungsbedingungen (english version: see below)

// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -

// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den

// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool 

// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.

// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.

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

// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im

// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.

// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie

// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.

// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren

// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.

// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren

// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand 

// + des Mitverschuldens offen.

// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.

// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.

// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.

// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang

// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.

// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.

// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'

// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.

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

// + Software LICENSING TERMS

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

// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -

// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware 

// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.

// + The Software may only be used with the Licensor's products.

// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this

// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this

// + agreement shall be the property of the Licensor.

// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other

// + features that can be used to identify the program may not be altered or defaced by the customer.

// + The customer shall be responsible for taking reasonable precautions

// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the

// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and

// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product

// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.

// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test

// + the software for his purpose before any operational usage. The customer will backup his data before using the software.

// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data

// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.

// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.

// + #### END OF LICENSING TERMS ####

// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.

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

#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 StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;

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

char MotorenEin = 0,StartTrigger = 0;

long HoehenWert = 0;

long SollHoehe = 0;

signed int AltitudeSetpointTrimming = 0;

long FromNC_AltitudeSetpoint = 0;

unsigned char FromNC_AltitudeSpeed = 0;

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

signed char WaypointTrimming = 0;

int CompassGierSetpoint = 0;

unsigned char CalibrationDone = 0;

char NeueKompassRichtungMerken = 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_MaximumAltitude;

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 = ' ';

unsigned int HooverGasEmergencyPercent = 0; // The gas value for Emergency landing

#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] = ErsatzKompassInGrad;

    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!

GRN_OFF;

 while(Anzahl--)

 {

  beeptime = dauer;

  while(beeptime);

  Delay_ms(dauer * 2);

 }

GRN_ON;

}

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

// 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_IntegralNick = IntegralNick;

    Mess_IntegralRoll = 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;

    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;

#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))

        LIBFC_HoTT_Clear();

#endif

}

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

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

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

    {

     int tmp2;

         tmp = EE_Parameter.Kanalbelegung[K_POTI1 + i];

         tmp2 = PPM_in[tmp] + 127;

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

         if(tmp == 25) Poti[i] = tmp2; // 25 = WaypointEvent channel -> no filter

         else

     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,5,255);

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

 if(EE_Parameter.Servo3 == 247) { if(PORTC & (1<<PORTC2)) Parameter_Servo3 = 140; else Parameter_Servo3 = 70;}       // Out1 (J16)

 else if(EE_Parameter.Servo3 == 246) { if(PORTC & (1<<PORTC3)) Parameter_Servo3 = 140; else Parameter_Servo3 = 70;}

 else CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3);

 if(EE_Parameter.Servo4 == 247) { if(PORTC & (1<<PORTC2)) Parameter_Servo4 = 140; else Parameter_Servo4 = 70;}

 else if(EE_Parameter.Servo4 == 246) { if(PORTC & (1<<PORTC3)) Parameter_Servo4 = 140; else Parameter_Servo4 = 70;}  // Out2 (J17)

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

 CHK_POTI(Parameter_MaximumAltitude,EE_Parameter.MaxAltitude);

 if((NC_To_FC_MaxAltitude && NC_To_FC_MaxAltitude < Parameter_MaximumAltitude) || Parameter_MaximumAltitude == 0) Parameter_MaximumAltitude = NC_To_FC_MaxAltitude;

 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.CareFreeModeControl;

 if(tmp > 50)

   {

        CareFree = 1;

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

    if(carefree_old != CareFree)

    {

      if(carefree_old < 3)

           {

                if(CareFree) beeptime = 1500;