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,RohMesswertNick,RohMesswertRoll;

int TrimNick, TrimRoll;

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

int BoatNeutralNick = 0,BoatNeutralRoll = 0,BoatNeutralGier = 0;

int Mittelwert_AccNick, Mittelwert_AccRoll;

unsigned int NeutralAccX=0, NeutralAccY=0;

int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;

int NeutralAccZ = 0;

signed char NeutralAccZfine = 0;

unsigned char ControlHeading = 0;// in 2°

long IntegralNick = 0;

long IntegralRoll = 0;

long Integral_Gier = 0;

long Mess_IntegralNick = 0;

long Mess_IntegralRoll = 0;

long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;

long MittelIntegralNick,MittelIntegralRoll;

long SummeNick=0,SummeRoll=0;

volatile long Mess_Integral_Hoch = 0;

int  KompassValue = -1;

int  KompassSollWert = 0;

int  NC_CompassSetpoint = -1;

//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, CompassCorrected; // Kompasswert in Grad

int   GierGyroFehler = 0;

char GyroFaktor,GyroFaktorGier;

char IntegralFaktor,IntegralFaktorGier;

int  DiffNick,DiffRoll;

unsigned int StickGasHover = 127;

int 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 = 0;      // 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_TiltCompensation = 100;        // 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_NickControl = 100;

unsigned char Parameter_ServoNickControl = 100;

unsigned char Parameter_ServoRollControl = 100;

unsigned char Parameter_ServoNickComp = 50;

unsigned char Parameter_ServoRollComp = 85;

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

unsigned char Parameter_NaviGpsP;

unsigned char Parameter_NaviGpsI;

unsigned char Parameter_NaviGpsD;

unsigned char Parameter_NaviGpsA;

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

int MaxStickNick = 0,MaxStickRoll = 0;

unsigned int  modell_fliegt = 0;

volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0;

unsigned char FC_StatusFlags3 = 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

unsigned int GasIsZeroCnt = 0; // to detect that the gas-stick is down for a while

signed int Variance = 0;

signed int CosAttitude; // for projection of hoover gas

unsigned char ACC_AltitudeControl = 0;

unsigned char LowVoltageLandingActive = 0;

unsigned char LowVoltageHomeActive = 0;

signed int DriftNick = 0, DriftRoll = 0;

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

#define OPA_OFFSET_STEP 5

#else

#define OPA_OFFSET_STEP 10

#endif

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

//  Debugwerte zuordnen

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

void CopyDebugValues(void)

{

    DebugOut.Analog[0] = ToNaviCtrl.IntegralNick;//IntegralNick / (EE_Parameter.GyroAccFaktor * 4);

    DebugOut.Analog[1] = ToNaviCtrl.IntegralRoll;//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/10;

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

    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[21] = HoverGas;

    DebugOut.Analog[22] = Capacity.ActualCurrent;

    DebugOut.Analog[23] = Capacity.UsedCapacity;

        DebugOut.Analog[24] = SollHoehe/10;    

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

//DebugOut.Analog[16] = MinBlTemperture; 

//DebugOut.Analog[17] = MaxBlTemperture; 

//DebugOut.Analog[16] = Variance;

//DebugOut.Analog[17] = VarioMeter;

//DebugOut.Analog[16] = GasIsZeroCnt;

//DebugOut.Analog[18] = HoehenWertF;            

//DebugOut.Analog[25] = Parameter_Hoehe_P;

//DebugOut.Analog[26] = Parameter_Luftdruck_D;

}

void Piep(unsigned char Anzahl, unsigned int dauer)

{

 unsigned int wait = 0;

 if(MotorenEin) return; //auf keinen Fall im Flug!

GRN_OFF;

 while(Anzahl--)

 {

  beeptime = dauer;

  wait = dauer;

  while(beeptime || wait)

   {

    if(UpdateMotor)

         {

          UpdateMotor = 0;

          if(!beeptime) wait--;

          LIBFC_Polling();

         };

   }

 }

GRN_ON;

}

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

// Messwerte beim Ermitteln der Nullage

void CalibrierMittelwert(void)

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

{

//    if(PlatinenVersion == 13) SucheGyroOffset();

    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern

        ANALOG_OFF;

        MesswertNick = AdWertNick;

        MesswertRoll = AdWertRoll;

        MesswertGier = AdWertGier;

        Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick;

        Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll;

   // ADC einschalten

    ANALOG_ON;

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

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

}

void StoreNeutralToEeprom(void)

{

        BoatNeutralNick = AdNeutralNick;

        BoatNeutralRoll = AdNeutralRoll;

        BoatNeutralGier = AdNeutralGier;

        SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX);

        SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY);

        SetParamWord(PID_GYRO_NICK,(uint16_t)BoatNeutralNick);

        SetParamWord(PID_GYRO_ROLL,(uint16_t)BoatNeutralRoll);

        SetParamWord(PID_GYRO_YAW,(uint16_t)BoatNeutralGier);

}

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

//  Nullwerte ermitteln

//  Parameter: 0 -> after switch on (ignore ACC-Z fault)

//  Parameter: 1 -> before Start

//  Parameter: 2 -> calibrate and store ACC

//  Parameter: 3 -> use stored Gyro calibration Data from EEPROM

unsigned char SetNeutral(unsigned char AdjustmentMode)  // retuns: "sucess"

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

{

        unsigned char i, sucess = 1;

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

    VersionInfo.HardwareError[0] = 0;

//    HEF4017Reset_ON;

        NeutralAccX = 0;

        NeutralAccY = 0;

        NeutralAccZ = 0;

        NeutralAccZfine = 0;

    AdNeutralNick = 0;

        AdNeutralRoll = 0;

        AdNeutralGier = 0;

    Parameter_AchsKopplung1 = 0;

    Parameter_AchsKopplung2 = 0;

    ExpandBaro = 0;

    if(AdjustmentMode == 3)  FC_StatusFlags3 |= FC_STATUS3_BOAT;

        // else FC_StatusFlags3 &= ~FC_STATUS3_BOAT; -> do not clear that

    CalibrierMittelwert();

    Delay_ms_Mess(100);

        CalibrierMittelwert();

    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?

     {

      if((MessLuftdruck > 950) || (MessLuftdruck < 750) || ExpandBaro) SucheLuftruckOffset();

     }

        barotest = MessLuftdruck;

#define NEUTRAL_FILTER 32

        OCR0A += OPA_OFFSET_STEP;

        OCR0B = 255 - OCR0A;

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

         {

          Delay_ms_Mess(10);

          gier_neutral += AdWertGier;

          nick_neutral += AdWertNick;

          roll_neutral += AdWertRoll;

          acc_z_neutral += Aktuell_az;

         }

        if(MessLuftdruck < 1010 && MessLuftdruck > 20) BaroStep = barotest - MessLuftdruck;

        OCR0A -= OPA_OFFSET_STEP;

        OCR0B = 255 - OCR0A;

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

     NeutralAccZ = (acc_z_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);

     StartNeutralRoll = AdNeutralRoll;

     StartNeutralNick = AdNeutralNick;

     if(AdjustmentMode == 2)

     {

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

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

                // Save ACC neutral settings to eeprom

                StoreNeutralToEeprom();

    }

    else

    {

                // restore from eeprom

                NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK);

                NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL);

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

                        sucess = 0;

                }

        // restore from eeprom

        BoatNeutralNick = (int16_t)GetParamWord(PID_GYRO_NICK);

        BoatNeutralRoll = (int16_t)GetParamWord(PID_GYRO_ROLL);

        BoatNeutralGier = (int16_t)GetParamWord(PID_GYRO_YAW);

     if(FC_StatusFlags3 & FC_STATUS3_BOAT) // Read Gyro Data from eeprom

     {

                // strange settings?

                if(((unsigned int) BoatNeutralNick > (600 * 16)) || ((unsigned int) BoatNeutralNick < (400 * 16))

                   || ((unsigned int) BoatNeutralRoll > (600 * 16)) || ((unsigned int) BoatNeutralRoll < (400 * 16))

                   || ((unsigned int) BoatNeutralGier > (600 * 2)) || ((unsigned int) BoatNeutralGier < (400 * 2)))

                {

                        printf("\n\rGyro calibration data not valid\r\n");

                        sucess = 0;

                        FC_StatusFlags3 &= ~FC_STATUS3_BOAT;

                }

                else

                {

                        AdNeutralNick = BoatNeutralNick;

                        AdNeutralRoll = BoatNeutralRoll;

                        AdNeutralGier = BoatNeutralGier;

                }

     }

    }

        EEAR = EE_DUMMY;  // Set the EEPROM Address pointer to an unused space

        MesswertNick = 0;

    MesswertRoll = 0;

    MesswertGier = 0;

    Delay_ms_Mess(200);

    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;

    KompassSollWert = KompassValue;

        NC_CompassSetpoint = -1;

        KompassSignalSchlecht = 100;

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

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

    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;

    GierGyroFehler = 0;

    LED_Init();

    if(AdjustmentMode != 0) FC_StatusFlags |= FC_STATUS_CALIBRATE;

    FromNaviCtrl_Value.Kalman_K = -1;

    FromNaviCtrl_Value.Kalman_MaxDrift = 0;

    FromNaviCtrl_Value.Kalman_MaxFusion = 32;

    SenderOkay = 100;

    if(ServoActive)     DDRD  |=0x80; // enable J7 -> Servo signal

        else

         {

      if((EE_Parameter.ServoCompInvert & SERVO_NICK_INV) && (EE_Parameter.ServoCompInvert & SERVO_RELATIVE)) NickServoValue = 12000;//((128 + 60) * 4 * 16); // neutral position = upper 1/4//    else    

          else NickServoValue = ((128 - 60) * 4 * 16); // neutral position = lower 1/4

          CalculateServoSignals = 1;

          CalculateServo(); // nick

          CalculateServo(); // roll

         }

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

    signed int tilt1, tilt2;

                tilt1 = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg

                tilt2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR);  // roll angle in deg

                tilt1 = (int16_t)ihypot(tilt1,tilt2);                   // tilt angle over all 

                CosAttitude = c_cos_8192(tilt1);                               

                NeutralAccZ = (long)((long) (NeutralAccZ - 512) * 8192 + 4096) / CosAttitude + 512;

                if(tilt1 > 20) sucess = 0; // calibration must be within 20° Tilt angle 

                if(AdjustmentMode != 0 && ACC_AltitudeControl) if((NeutralAccZ < 682 - 30) || (NeutralAccZ > 682 + 35)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; sucess = 0;};

#else

        NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP);

        EEAR = EE_DUMMY;  // Set the EEPROM Address pointer to an unused space

#endif

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

    if(VersionInfo.HardwareError[0]) sucess = 0;

    carefree_old = 70;

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

        LIBFC_HoTT_Clear();

        ACC_AltitudeFusion(2); // initalisation

#endif

    StartLuftdruck = Luftdruck;

    VarioMeter = 0;

        SummenHoehe = 0;    Mess_Integral_Hoch = 0;

        DebugOut.Analog[28] = 0; // I2C-Counter

        CalcExpandBaroStep();

        if(FC_StatusFlags3 & FC_STATUS3_BOAT && !EE_Parameter.Driftkomp) EE_Parameter.Driftkomp = 4;

        ShutterCounter = 0;

/*

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

//For testing the expandBaro at 30m                            

                                ExpandBaro -= 1;

                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down

                                OCR0B = 255 - OCR0A;

                                CalcExpandBaroStep();

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

*/

 return(sucess);

}

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

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

    NaviAccNick    += AdWertAccNick;

    NaviAccRoll    += AdWertAccRoll;

    NaviCntAcc++;

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

        HoehenWert = HoehenWert_Mess;

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

        HoehenWertF = HoehenWertF_Mess;

#else

        HoehenWertF = HoehenWert;

#endif

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

// 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_IntegralRoll +=  MesswertRoll + TrimRoll - LageKorrekturRoll;

            if(Mess_IntegralRoll > Umschlag180Roll)

            {

             Mess_IntegralRoll  = -(Umschlag180Roll - 25000L);

            }

            if(Mess_IntegralRoll <-Umschlag180Roll)

            {

             Mess_IntegralRoll =  (Umschlag180Roll - 25000L);

            }

// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++

            Mess_IntegralNick  += MesswertNick + TrimNick - LageKorrekturNick;

            if(Mess_IntegralNick > Umschlag180Nick)

             {

              Mess_IntegralNick = -(Umschlag180Nick - 25000L);

             }

            if(Mess_IntegralNick <-Umschlag180Nick)

            {

             Mess_IntegralNick =  (Umschlag180Nick - 25000L);

            }

    Integral_Gier  = Mess_Integral_Gier;

    IntegralNick = Mess_IntegralNick;

    IntegralRoll = Mess_IntegralRoll;

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

                 FC_StatusFlags2 &= ~FC_STATUS2_WAIT_FOR_TAKEOFF;

                 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;

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

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

#ifdef REDUNDANT_FC_SLAVE

        I2CTimeout = 100;

        return; // don't send I2C-Data

#endif

#ifdef REDUNDANT_FC_MASTER

if(Parameter_UserParam7 > 150)

 {

        FC_StatusFlags3 |= FC_STATUS3_REDUNDANCE_TEST;

        // Make noise

        if((BeepMuster == 0xffff)) { beeptime = 25000; BeepMuster = 0x0080; }