Subversion Repositories FlightCtrl

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

Flight Control

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

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

// + Copyright (c) 04.2007 Holger Buss

// + Nur für den privaten Gebrauch

// + www.MikroKopter.com

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

// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),

// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.

// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt

// + bzgl. der Nutzungsbedingungen aufzunehmen.

// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,

// + Verkauf von Luftbildaufnahmen, usw.

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

// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,

// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen

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

// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts

// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"

// + eindeutig als Ursprung verlinkt werden

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

// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion

// + Benutzung auf eigene Gefahr

// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden

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

// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur

// + mit unserer Zustimmung zulässig

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

// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen

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

// + Redistributions of source code (with or without modifications) must retain the above copyright notice,

// + this list of conditions and the following disclaimer.

// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived

// +     from this software without specific prior written permission.

// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet

// +     for non-commercial use (directly or indirectly)

// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted

// +     with our written permission

// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be

// +     clearly linked as origin

// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed

// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

// +  POSSIBILITY OF SUCH DAMAGE.

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

#include "main.h"

#include "eeprom.c"

#include "parameter.h"

unsigned char h,m,s;

volatile unsigned int I2CTimeout = 100;

volatile int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias;

int AdNeutralGierBias;

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

int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;

int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;

volatile float NeutralAccZ = 0;

unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;

long IntegralNick = 0,IntegralNick2 = 0;

long IntegralRoll = 0,IntegralRoll2 = 0;

long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;

long Integral_Gier = 0;

long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;

long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;

long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;

long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;

volatile long Mess_Integral_Hoch = 0;

volatile int  KompassValue = 0;

volatile int  KompassStartwert = 0;

volatile int  KompassRichtung = 0;

unsigned int  KompassSignalSchlecht = 500;

unsigned char  MAX_GAS,MIN_GAS;

unsigned char Notlandung = 0;

unsigned char HoehenReglerAktiv = 0;

unsigned char TrichterFlug = 0;

long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;

long  ErsatzKompass;

int   ErsatzKompassInGrad; // Kompasswert in Grad

int   GierGyroFehler = 0;

float GyroFaktor;

float IntegralFaktor;

volatile int  DiffNick,DiffRoll;

int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;

volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;

volatile unsigned char SenderOkay = 0;

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

char MotorenEin = 0;

int HoehenWert = 0;

int SollHoehe = 0;

int LageKorrekturRoll = 0,LageKorrekturNick = 0;

float Ki =  FAKTOR_I;

unsigned char Looping_Nick = 0,Looping_Roll = 0;

unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;

unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250

unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250

unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32

unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250

unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250

unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250

unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250

unsigned char Parameter_Gier_P = 2;             // Wert : 1-20

unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20

unsigned char Parameter_UserParam1 = 0;

unsigned char Parameter_UserParam2 = 0;

unsigned char Parameter_UserParam3 = 0;

unsigned char Parameter_UserParam4 = 0;

unsigned char Parameter_UserParam5 = 0;

unsigned char Parameter_UserParam6 = 0;

unsigned char Parameter_UserParam7 = 0;

unsigned char Parameter_UserParam8 = 0;

unsigned char Parameter_ServoNickControl = 100;

unsigned char Parameter_LoopGasLimit = 70;

unsigned char Parameter_AchsKopplung1 = 0;

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_J16Brightness; // for the J16 Output

unsigned char Parameter_J17Bitmask;             // for the J17 Output

unsigned char Parameter_J17Timing;              // for the J17 Output

unsigned char Parameter_J17Brightness; // for the J17 Output

unsigned char Parameter_NaviGpsModeControl;     // Parameters for the Naviboard

unsigned char Parameter_NaviGpsGain;

unsigned char Parameter_NaviGpsP;

unsigned char Parameter_NaviGpsI;

unsigned char Parameter_NaviGpsD;

unsigned char Parameter_NaviGpsACC;

unsigned char Parameter_NaviOperatingRadius;

unsigned char Parameter_NaviWindCorrection;

unsigned char Parameter_NaviSpeedCompensation;

unsigned char Parameter_ExternalControl;

struct mk_param_struct EE_Parameter;

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

int MaxStickNick = 0,MaxStickRoll = 0;

unsigned int  modell_fliegt = 0;

unsigned char MikroKopterFlags = 0;

void Piep(unsigned char Anzahl)

{

 while(Anzahl--)

 {

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

  beeptime = 100;

  Delay_ms(250);

 }

}

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

//  Nullwerte ermitteln

void SetNeutral(void)

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

{

        NeutralAccX = 0;

        NeutralAccY = 0;

        NeutralAccZ = 0;

    AdNeutralNick = 0;

        AdNeutralRoll = 0;

        AdNeutralGier = 0;

    AdNeutralGierBias = 0;

    Parameter_AchsKopplung1 = 0;

    Parameter_AchsGegenKopplung1 = 0;

    ExpandBaro = 0;

    CalibrierMittelwert();

    Delay_ms_Mess(100);

        CalibrierMittelwert();

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

     {

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

     }

     AdNeutralNick= AdWertNick;

     AdNeutralRoll= AdWertRoll;

     AdNeutralGier= AdWertGier;

     AdNeutralGierBias = AdWertGier;

     StartNeutralRoll = AdNeutralRoll;

     StartNeutralNick = AdNeutralNick;

    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)

    {

          NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;

          NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;

          NeutralAccZ = Aktuell_az;

    }

    else

    {

          NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);

          NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);

          NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);

    }

    Mess_IntegralNick = 0;

    Mess_IntegralNick2 = 0;

    Mess_IntegralRoll = 0;

    Mess_IntegralRoll2 = 0;

    Mess_Integral_Gier = 0;

    MesswertNick = 0;

    MesswertRoll = 0;

    MesswertGier = 0;

 Delay_ms_Mess(100);

    StartLuftdruck = Luftdruck;

    HoeheD = 0;

    Mess_Integral_Hoch = 0;

    KompassStartwert = KompassValue;

    GPS_Neutral();

    beeptime = 50;

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

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

    ExternHoehenValue = 0;

    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;

    GierGyroFehler = 0;

    SendVersionToNavi = 1;

    LED_Init();

    MikroKopterFlags |= FLAG_CALIBRATE;

    FromNaviCtrl_Value.Kalman_K = -1;

    FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16;

    FromNaviCtrl_Value.Kalman_MaxFusion = 32;

}

inline void LesePotis(void) {

    /*  Warum 110? Knüppel geht von -125 bis 125!

        if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;

        if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;

        if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;

        if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;

     */

    if (Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 125) Poti1++;

    else if (Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 125 && Poti1) Poti1--;

    if (Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 125) Poti2++;

    else if (Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 125 && Poti2) Poti2--;

    if (Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 125) Poti3++;

    else if (Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 125 && Poti3) Poti3--;

    if (Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 125) Poti4++;

    else if (Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 125 && Poti4) Poti4--;

    if (Poti1 < 0) Poti1 = 0;

    else if (Poti1 > 255) Poti1 = 255;

    if (Poti2 < 0) Poti2 = 0;

    else if (Poti2 > 255) Poti2 = 255;

    if (Poti3 < 0) Poti3 = 0;

    else if (Poti3 > 255) Poti3 = 255;

    if (Poti4 < 0) Poti4 = 0;

    else if (Poti4 > 255) Poti4 = 255;

}

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

// Bearbeitet die Messwerte

inline void Mittelwert(void)

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

{

    static signed long tmpl,tmpl2;

    MesswertGier = (signed int) AdNeutralGier - AdWertGier;

    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;

    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;

    MesswertNick = (signed int) AdWertNick - AdNeutralNick;

//DebugOut.Analog[26] = MesswertNick;

//DebugOut.Analog[28] = MesswertRoll;

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

        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;

        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;

        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;

    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;

    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;

    NaviAccNick    += AdWertAccNick;

    NaviAccRoll    += AdWertAccRoll;

    NaviCntAcc++;

    IntegralAccZ    += Aktuell_az - NeutralAccZ;

// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++

            ErsatzKompass +=  MesswertGier;

            Mess_Integral_Gier +=  MesswertGier;

//            Mess_Integral_Gier2 += MesswertGier;

                    if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360? Umschlag

                    if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;

// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++

      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))

         {

            tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L;

            tmpl *= Parameter_AchsKopplung1;  //125

            tmpl /= 4096L;

            tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L;

            tmpl2 *= Parameter_AchsKopplung1;

            tmpl2 /= 4096L;

            if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;

         }

      else  tmpl = tmpl2 = 0;

// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++

            MesswertRoll += tmpl;

            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109

            Mess_IntegralRoll2 += MesswertRoll;

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

            }

            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 > 2020) MesswertRoll = +1000;

              if(AdWertRoll > 2034) MesswertRoll = +2000;

                         }

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

            MesswertNick -= tmpl2;

            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;

            Mess_IntegralNick2 += MesswertNick;

            Mess_IntegralNick  += MesswertNick - 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;

            }

            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 > 2020) MesswertNick = +1000;

              if(AdWertNick > 2034) MesswertNick = +2000;

                         }

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

// ADC einschalten

    ANALOG_ON;

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

    Integral_Gier  = Mess_Integral_Gier;

    IntegralNick = Mess_IntegralNick;

    IntegralRoll = Mess_IntegralRoll;

    IntegralNick2 = Mess_IntegralNick2;

    IntegralRoll2 = Mess_IntegralRoll2;

  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)

  {

    if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);

    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);

    if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);

    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);

  }

    LesePotis();

}

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

// Messwerte beim Ermitteln der Nullage

void CalibrierMittelwert(void)

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

{

    if(PlatinenVersion == 13) SucheGyroOffset();

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

        ANALOG_OFF;

        MesswertNick = AdWertNick;

        MesswertRoll = AdWertRoll;

        MesswertGier = AdWertGier;

        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;

        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;

        Mittelwert_AccHoch = (long)AdWertAccHoch;

   // ADC einschalten

    ANALOG_ON;

        LesePotis();

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

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

}

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

// Senden der Motorwerte per I2C-Bus

void SendMotorData(void)

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

{

    if (!(MotorenEin && !PARAM_ENGINE_DISABLED)) {

        Motor_Hinten = 0;

        Motor_Vorne = 0;

        Motor_Rechts = 0;

        Motor_Links = 0;

        if(MotorTest[0]) Motor_Vorne = MotorTest[0];

        if(MotorTest[1]) Motor_Hinten = MotorTest[1];

        if(MotorTest[2]) Motor_Links = MotorTest[2];

        if(MotorTest[3]) Motor_Rechts = MotorTest[3];

        MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);

    } else MikroKopterFlags |= FLAG_MOTOR_RUN;

    DebugOut.Analog[12] = Motor_Vorne;

    DebugOut.Analog[13] = Motor_Hinten;

    DebugOut.Analog[14] = Motor_Links;

    DebugOut.Analog[15] = Motor_Rechts;

    //Start I2C Interrupt Mode

    twi_state = 0;

    motor = 0;

    i2c_start();

}

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

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

void ParameterZuordnung(void)

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

{

 #define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}

 #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; }

 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);

 CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);

 CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);

 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);

 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);

 CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);

 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);

 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);

 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);

 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);

 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);

 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);

/*

 CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);

 CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);

*/

    CHK_POTI_MM(Parameter_J16Brightness, PARAM_LED_BRIGHTNESS_J16, 0, 250);

    CHK_POTI_MM(Parameter_J17Brightness, PARAM_LED_BRIGHTNESS_J17, 0, 250);

 CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);

 CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);

 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);

 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);

 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);

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

 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);

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

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

// CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);

 //CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);

// CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);

// CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);

// CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);

// CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);

// CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255);

// CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255);

// CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255);

 CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);

 Ki = (float) Parameter_I_Faktor * 0.0001;

 MAX_GAS = EE_Parameter.Gas_Max;

 MIN_GAS = EE_Parameter.Gas_Min;

}

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

//

void MotorRegler(void)

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

{

         int motorwert,pd_ergebnis,h,tmp_int;

         int GierMischanteil,GasMischanteil;

     static long SummeNick=0,SummeRoll=0;

     static long sollGier = 0,tmp_long,tmp_long2;

     static long IntegralFehlerNick = 0;

     static long IntegralFehlerRoll = 0;

         static unsigned int RcLostTimer;

         static unsigned char delay_neutral = 0;

         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;

     static int hoehenregler = 0;

     static char TimerWerteausgabe = 0;

     static char NeueKompassRichtungMerken = 0;

     static long ausgleichNick, ausgleichRoll;

        Mittelwert();

    GRN_ON;

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

// Gaswert ermitteln

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

        GasMischanteil = StickGas;

    if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;

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

// Empfang schlecht

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

   if(SenderOkay < 100)

        {

        if(!PcZugriff)

         {

           if(BeepMuster == 0xffff)

            {

             beeptime = 15000;

             BeepMuster = 0x0c00;

            }

         }

        if(RcLostTimer) RcLostTimer--;

        else

         {

          MotorenEin = 0;

          Notlandung = 0;

         }

        ROT_ON;

        if(modell_fliegt > 1000)  // wahrscheinlich in der Luft --> langsam absenken

            {

            GasMischanteil = EE_Parameter.NotGas;

            Notlandung = 1;

            PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;

            PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;

            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;

            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;

            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;

            }

         else MotorenEin = 0;

        }

        else

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

// Emfang gut

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

        if(SenderOkay > 140)

            {

            Notlandung = 0;

            RcLostTimer = EE_Parameter.NotGasZeit * 50;

            if(GasMischanteil > 40 && MotorenEin)

                {

                if(modell_fliegt < 0xffff) modell_fliegt++;

                }

            if((modell_fliegt < 256))

                {

                SummeNick = 0;

                SummeRoll = 0;

                if(modell_fliegt == 250)

                 {

                  NeueKompassRichtungMerken = 1;

                  sollGier = 0;

                  Mess_Integral_Gier = 0;

//                  Mess_Integral_Gier2 = 0;

                 }

                } else MikroKopterFlags |= FLAG_FLY;

            if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && !MotorenEin)

                {

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

// auf Nullwerte kalibrieren

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

                if(!MotorenEin && PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte

                    {

                    if(++delay_neutral > 200)  // nicht sofort

                        {

                        GRN_OFF;

                        MotorenEin = 0;

                        delay_neutral = 0;

                        modell_fliegt = 0;

                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)

                        {

                         unsigned char setting=1;

                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;

                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;

                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;

                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;

                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;

                         SetActiveParamSetNumber(setting);  // aktiven Datensatz merken

                        }

//                        else

                         if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)

                          {

                           WinkelOut.CalcState = 1;

                           beeptime = 1000;

                          }

                          else

                          {

                               ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);

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

                            {

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

                            }

                           SetNeutral();

                           Piep(GetActiveParamSetNumber());

                         }

                        }

                    }

                 else

                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern

                    {

                    if(++delay_neutral > 200)  // nicht sofort

                        {

                        GRN_OFF;

                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte l?schen

                        MotorenEin = 0;

                        delay_neutral = 0;

                        modell_fliegt = 0;

                        SetNeutral();

                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern

                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern

                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);

                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);

                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);

                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);

                        Piep(GetActiveParamSetNumber());

                        }

                    }

                 else delay_neutral = 0;

                }

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

// Gas ist unten

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

            if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)

                {

                // Starten

                if((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) && !MotorenEin)

                    {

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

// Einschalten

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

                    if(++delay_einschalten > 200)

                        {

                        delay_einschalten = 200;

                        modell_fliegt = 1;

                        MotorenEin = 1;

                        sollGier = 0;

                        Mess_Integral_Gier = 0;

                        Mess_Integral_Gier2 = 0;

                        Mess_IntegralNick = 0;

                        Mess_IntegralRoll = 0;

                        Mess_IntegralNick2 = IntegralNick;

                        Mess_IntegralRoll2 = IntegralRoll;

                        SummeNick = 0;

                        SummeRoll = 0;

                        MikroKopterFlags |= FLAG_START;

                    // Beim Einschalten automatisch kalibrieren

                    if (PARAM_CAL_ON_START) {

                        if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) {

                            if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) {

                                SucheLuftruckOffset();

                            }

                        }

                        SetNeutral();

                    }

                        }

                    }

                    else delay_einschalten = 0;

                //Auf Neutralwerte setzen

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

// Auschalten

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

                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)

                    {

                    if(++delay_ausschalten > 200)  // nicht sofort

                        {

                        MotorenEin = 0;

                        delay_ausschalten = 200;

                        modell_fliegt = 0;

                        }

                    }

                else delay_ausschalten = 0;

                }

            }

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

// neue Werte von der Funke

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

 if(!NewPpmData-- || Notlandung)

  {

        static int chanNickPrev = 0;

        static int chanRollPrev = 0;

        static int stick_nick,stick_roll;

        ParameterZuordnung();

#define MAX_CHAN_VAL 125L

#define COS45        7071L              // cos( -45 ) * 10000

        long chanNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];

        long chanRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];

        int chanNickDiff;

        int chanRollDiff;

        /* Über Parameter läßt sich zwischen "+" und "X" - Formations

         * umschalten (sh. parameter.h)

         */

        if (PARAM_X_FORMATION) {

            // Stick-Koordinatensystem um -45° (rechts) drehen

            chanNick *= COS45;

            chanRoll *= -COS45;

            int chanNickTemp = (chanNick - chanRoll) / 10000L;