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 HoverGasEmergencyPercent = 0; // The gas value for Emergency landing

static int HoehenRegelung(int GasMischanteil);

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

        //    HEF4017Reset_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)

        {

                //              HEF4017Reset_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 (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))

                                        if(CareFree)

                                        { beeptime = 1500;  if(!SpeakHoTT) SpeakHoTT = SPEAK_CF_ON; }

                                        else

                                        { beeptime = 200;  if(!SpeakHoTT) SpeakHoTT = SPEAK_CF_OFF; }

                                #else

                                        if(CareFree)

                                                beeptime = 1500;

                                        else

                                                beeptime = 200;

                                #endif

                                NeueKompassRichtungMerken = 5;

                                carefree_old = CareFree;

                        } else carefree_old--;

                }  

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