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 "parameter.h"

#include "pitch.h"

#include "altcon.h"

#include "eeprom.c"

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;

}

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

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)

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

{

    DebugOut.Analog[12] = Motor_Vorne;

    DebugOut.Analog[13] = Motor_Hinten;

    DebugOut.Analog[14] = Motor_Links;

    DebugOut.Analog[15] = Motor_Rechts;

    if (!(MotorenEin && PARAM_ENGINE_ENABLED)) {

        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;

    //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_UserParam5, EE_Parameter.UserParam5, 0, 255);

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

    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_J16Brightness, PARAM_LED_BRIGHTNESS_J16, 0, 250);

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

    CHK_POTI_MM(Parameter_J17Brightness, PARAM_LED_BRIGHTNESS_J17, 0, 250);

    // 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, 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 char TimerWerteausgabe = 0;

    static char NeueKompassRichtungMerken = 0;

    static long ausgleichNick, ausgleichRoll;

    Mittelwert();

    GRN_ON;

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

    // Gaswert ermitteln

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

    GasMischanteil = StickGas;

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

    // 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 == 0) {

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

            // auf Nullwerte kalibrieren

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

            if (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 - 125) {

            // Starten

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

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

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

            chanRoll = -chanRoll;

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

            chanNick *= COS45;

            chanRoll *= COS45;

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

            int chanRollTemp = (chanRoll + chanNick) / 10000L;

            chanNick = chanNickTemp;

            chanRoll = -chanRollTemp;

            if (chanNick > MAX_CHAN_VAL)

                chanNick = MAX_CHAN_VAL;

            if (chanNick < -MAX_CHAN_VAL)

                chanNick = -MAX_CHAN_VAL;

            if (chanRoll > MAX_CHAN_VAL)

                chanRoll = MAX_CHAN_VAL;

            if (chanRoll < -MAX_CHAN_VAL)

                chanRoll = -MAX_CHAN_VAL;

        }