WebSVN - FlightCtrl - Diff - Rev 688 and 690 - /branches/V0.68d Code Redesign killagreg/fc.c



volatile unsigned int I2CTimeout = 100;
volatile int MesswertNick,MesswertRoll,MesswertGier;
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
volatile float NeutralAccZ = 0;
 
volatile long IntegralNick = 0,IntegralNick2 = 0;
volatile long IntegralRoll = 0,IntegralRoll2 = 0;
volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
volatile long Integral_Gier = 0;
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;

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

    Parameter_AchsKopplung1 = 0;
    Parameter_AchsGegenKopplung1 = 0;
    CalibrierMittelwert();
    Delay_ms_Mess(100);
        CalibrierMittelwert();
    if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL))  // Höhenregelung aktiviert?
     {
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
     }

 

    IntegralAccZ    += Current_AccZ - NeutralAccZ;
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
            Mess_Integral_Gier +=  MesswertGier;
            Mess_Integral_Gier2 += MesswertGier;
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
      if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
         {
            tmpl = Mess_IntegralNick / 4096L;
            tmpl *= MesswertGier;
            tmpl *= Parameter_AchsKopplung1;  //125
            tmpl /= 2048L;

    IntegralNick = Mess_IntegralNick;
    IntegralRoll = Mess_IntegralRoll;
    IntegralNick2 = Mess_IntegralNick2;
    IntegralRoll2 = Mess_IntegralRoll2;

 
  if(ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER && !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);
  }
    if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
    if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
    if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
    if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && 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;

        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdValueAccNick;
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdValueAccRoll;
        Mittelwert_AccHoch = (long)AdValueAccTop;
   // ADC einschalten
    ADC_Enable();
    if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
    if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
    if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
    if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && 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;

 CHK_POTI(Parameter_Hoehe_P,ParamSet.Hoehe_P,0,100);
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,ParamSet.Hoehe_ACC_Wirkung,0,255);
 CHK_POTI(Parameter_KompassWirkung,ParamSet.KompassWirkung,0,255);
 CHK_POTI(Parameter_Gyro_P,ParamSet.Gyro_P,10,255);
 CHK_POTI(Parameter_Gyro_I,ParamSet.Gyro_I,0,255);
 CHK_POTI(Parameter_I_Factor,ParamSet.I_Factor,0,255);
 CHK_POTI(Parameter_UserParam1,ParamSet.UserParam1,0,255);
 CHK_POTI(Parameter_UserParam2,ParamSet.UserParam2,0,255);
 CHK_POTI(Parameter_UserParam3,ParamSet.UserParam3,0,255);
 CHK_POTI(Parameter_UserParam4,ParamSet.UserParam4,0,255);
 CHK_POTI(Parameter_UserParam5,ParamSet.UserParam5,0,255);

 CHK_POTI(Parameter_LoopGasLimit,ParamSet.LoopGasLimit,0,255);
 CHK_POTI(Parameter_AchsKopplung1,    ParamSet.AchsKopplung1,0,255);
 CHK_POTI(Parameter_AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255);
 CHK_POTI(Parameter_DynamicStability,ParamSet.DynamicStability,0,255);

 
 Ki = (float) Parameter_I_Factor * 0.0001;
 MAX_GAS = ParamSet.Gas_Max;
 MIN_GAS = ParamSet.Gas_Min;

          Notlandung = 0;
         }
        ROT_ON;
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
            {
            GasMischanteil = ParamSet.EmergencyGas;
            Notlandung = 1;
            PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0;
            PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
            PPM_in[ParamSet.ChannelAssignment[CH_GIER]] = 0;
            }
         else MotorenEin = 0;
        }
        else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang gut
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(SenderOkay > 140)
            {
            Notlandung = 0;
            RcLostTimer = ParamSet.EmergencyGasDuration * 50;
            if(GasMischanteil > 40)
                {
                if(modell_fliegt < 0xffff) modell_fliegt++;
                }
            if((modell_fliegt < 200) || (GasMischanteil < 40))

                SummeNick = 0;
                SummeRoll = 0;
                Mess_Integral_Gier = 0;
                Mess_Integral_Gier2 = 0;
                }
            if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorenEin == 0)
                {
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// auf Nullwerte kalibrieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] > 75)  // Neutralwerte
                    {
                    if(++delay_neutral > 200)  // nicht sofort
                        {
                        GRN_OFF;
                        MotorenEin = 0;
                        delay_neutral = 0;
                        modell_fliegt = 0;
                        if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70)
                        {
                         unsigned char setting=1;
                         if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 1;
                         if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 2;
                         if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 3;
                         if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 4;
                         if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 5;
                         SetActiveParamSet(setting);  // aktiven Datensatz merken
                        }
                        if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL))  // Höhenregelung aktiviert?
                          {
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
                          }
                            ParamSet_ReadFromEEProm(GetActiveParamSet());
                        SetNeutral();
                        Piep(GetActiveParamSet());
                        }
                    }
                 else
                if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] < -75)  // ACC Neutralwerte speichern
                    {
                    if(++delay_neutral > 200)  // nicht sofort
                        {
                        GRN_OFF;
                        SetParamByte(PID_ACC_NICK,0xFF); // Werte löschen

                 else delay_neutral = 0;
                }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gas ist unten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
            if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < 35-120)
                {
                // Starten
                if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] < -75)
                    {
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                    if(++delay_einschalten > 200)

                    else delay_einschalten = 0;
                //Auf Neutralwerte setzen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Auschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] > 75)
                    {
                    if(++delay_ausschalten > 200)  // nicht sofort
                        {
                        MotorenEin = 0;
                        delay_ausschalten = 200;

 if(!NewPpmData-- || Notlandung)
  {
    int tmp_int;
        static int stick_nick,stick_roll;
    ParameterZuordnung();
    StickNick = (StickNick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_P) / 4;
    StickNick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_D;
    StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
    StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
 
    StickGier = -PPM_in[ParamSet.ChannelAssignment[CH_GIER]];
        StickGas  = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;
 
   if(abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]) > MaxStickNick)
     MaxStickNick = abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]); else MaxStickNick--;
   if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll)
     MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); else MaxStickRoll--;
   if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}

 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;

    if(GyroFaktor < 0) GyroFaktor = 0;
    if(IntegralFaktor < 0) IntegralFaktor = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT)  Looping_Links = 1;
  else
   {
     {
      if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Links = 0;
     }
   }
  if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Rechts = 1;
   else
   {
   if(Looping_Rechts) // Hysterese
     {
      if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Rechts = 0;
     }
   }

 
  if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Oben = 1;
  else
   {
    if(Looping_Oben)  // Hysterese
     {
      if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Oben = 0;
     }
   }
  if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Unten = 1;
   else
   {
    if(Looping_Unten) // Hysterese
     {
      if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Unten = 0;
     }

   }
 

   if((MaxStickNick > 15) || (MaxStickRoll > 15))
    {
    tmp_long  /= 3;
    tmp_long2 /= 3;
    }
   if(abs(PPM_in[ParamSet.ChannelAssignment[CH_GIER]]) > 25)
    {
    tmp_long  /= 3;
    tmp_long2 /= 3;
    }

    ausgleichRoll = IntegralFehlerRoll / ParamSet.GyroAccAbgleich;

 
    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;

    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
 
   if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_GIER]]) > 25))
    {
     LageKorrekturNick /= 2;

             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
            }
           else last_n_n = 1;
         } else  last_n_n = 0;
        } else cnt = 0;
        if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;

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

        } else
        {
         cnt = 0;
        }

 
        if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
DebugOut.Analog[27] = ausgleichRoll;
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(abs(StickGier) > 20) // war 35
     {
      if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) NeueKompassRichtungMerken = 1;
     }
    tmp_int  = (long) ParamSet.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
    tmp_int += (ParamSet.Gier_P * StickGier) / 4;
    sollGier = tmp_int;
    Mess_Integral_Gier -= tmp_int;

    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(KompassValue && (ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE))
     {
       int w,v;
       static int SignalSchlecht = 0;
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln

// Höhenregelung
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//OCR0B = 180 - (Poti1 + 120) / 4;
//DruckOffsetSetting = OCR0B;
 if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL))  // Höhenregelung
  {
    int tmp_int;
    if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)  // Regler wird über Schalter gesteuert
    {
     if(Parameter_MaxHoehe < 50)
      {
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
       HoehenReglerAktiv = 0;

Rev 688	Rev 690
Line 69...	Line 69...
69	volatile unsigned int I2CTimeout = 100;	69	volatile unsigned int I2CTimeout = 100;
70	volatile int MesswertNick,MesswertRoll,MesswertGier;	70	volatile int MesswertNick,MesswertRoll,MesswertGier;
71	volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;	71	volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
72	volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;	72	volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
73	volatile float NeutralAccZ = 0;	73	volatile float NeutralAccZ = 0;
74	unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;	-
75	volatile long IntegralNick = 0,IntegralNick2 = 0;	74	volatile long IntegralNick = 0,IntegralNick2 = 0;
76	volatile long IntegralRoll = 0,IntegralRoll2 = 0;	75	volatile long IntegralRoll = 0,IntegralRoll2 = 0;
77	volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;	76	volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
78	volatile long Integral_Gier = 0;	77	volatile long Integral_Gier = 0;
79	volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;	78	volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
Line 110...	Line 109...
110	unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250	109	unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
111	unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250	110	unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250
112	unsigned char Parameter_Gyro_P = 150; // Wert : 10-250	111	unsigned char Parameter_Gyro_P = 150; // Wert : 10-250
113	unsigned char Parameter_Gyro_I = 150; // Wert : 0-250	112	unsigned char Parameter_Gyro_I = 150; // Wert : 0-250
114	unsigned char Parameter_Gier_P = 2; // Wert : 1-20	113	unsigned char Parameter_Gier_P = 2; // Wert : 1-20
115	unsigned char Parameter_I_Faktor = 10; // Wert : 1-20	114	unsigned char Parameter_I_Factor = 10; // Wert : 1-20
116	unsigned char Parameter_UserParam1 = 0;	115	unsigned char Parameter_UserParam1 = 0;
117	unsigned char Parameter_UserParam2 = 0;	116	unsigned char Parameter_UserParam2 = 0;
118	unsigned char Parameter_UserParam3 = 0;	117	unsigned char Parameter_UserParam3 = 0;
119	unsigned char Parameter_UserParam4 = 0;	118	unsigned char Parameter_UserParam4 = 0;
120	unsigned char Parameter_UserParam5 = 0;	119	unsigned char Parameter_UserParam5 = 0;
Line 154...	Line 153...
154	Parameter_AchsKopplung1 = 0;	153	Parameter_AchsKopplung1 = 0;
155	Parameter_AchsGegenKopplung1 = 0;	154	Parameter_AchsGegenKopplung1 = 0;
156	CalibrierMittelwert();	155	CalibrierMittelwert();
157	Delay_ms_Mess(100);	156	Delay_ms_Mess(100);
158	CalibrierMittelwert();	157	CalibrierMittelwert();
159	if((ParamSet.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?	158	if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Höhenregelung aktiviert?
160	{	159	{
161	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();	160	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();
162	}	161	}
Line 163...	Line 162...
163		162
Line 220...	Line 219...
220	IntegralAccZ += Current_AccZ - NeutralAccZ;	219	IntegralAccZ += Current_AccZ - NeutralAccZ;
221	// Gier ++++++++++++++++++++++++++++++++++++++++++++++++	220	// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
222	Mess_Integral_Gier += MesswertGier;	221	Mess_Integral_Gier += MesswertGier;
223	Mess_Integral_Gier2 += MesswertGier;	222	Mess_Integral_Gier2 += MesswertGier;
224	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++	223	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
225	if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))	224	if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
226	{	225	{
227	tmpl = Mess_IntegralNick / 4096L;	226	tmpl = Mess_IntegralNick / 4096L;
228	tmpl *= MesswertGier;	227	tmpl *= MesswertGier;
229	tmpl *= Parameter_AchsKopplung1; //125	228	tmpl *= Parameter_AchsKopplung1; //125
230	tmpl /= 2048L;	229	tmpl /= 2048L;
Line 297...	Line 296...
297	IntegralNick = Mess_IntegralNick;	296	IntegralNick = Mess_IntegralNick;
298	IntegralRoll = Mess_IntegralRoll;	297	IntegralRoll = Mess_IntegralRoll;
299	IntegralNick2 = Mess_IntegralNick2;	298	IntegralNick2 = Mess_IntegralNick2;
300	IntegralRoll2 = Mess_IntegralRoll2;	299	IntegralRoll2 = Mess_IntegralRoll2;
Line 301...	Line 300...
301		300
302	if(ParamSet.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)	301	if(ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER && !Looping_Nick && !Looping_Roll)
303	{	302	{
304	if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);	303	if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);
305	else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);	304	else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
306	if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200);	305	if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200);
307	else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);	306	else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
308	}	307	}
309	if(Poti1 < PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;	308	if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
310	if(Poti2 < PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;	309	if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
311	if(Poti3 < PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;	310	if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
312	if(Poti4 < PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;	311	if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
313	if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;	312	if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
314	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;	313	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
315	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;	314	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
316	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;	315	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
Line 329...	Line 328...
329	Mittelwert_AccNick = ACC_AMPLIFY * (long)AdValueAccNick;	328	Mittelwert_AccNick = ACC_AMPLIFY * (long)AdValueAccNick;
330	Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdValueAccRoll;	329	Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdValueAccRoll;
331	Mittelwert_AccHoch = (long)AdValueAccTop;	330	Mittelwert_AccHoch = (long)AdValueAccTop;
332	// ADC einschalten	331	// ADC einschalten
333	ADC_Enable();	332	ADC_Enable();
334	if(Poti1 < PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;	333	if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
335	if(Poti2 < PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;	334	if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
336	if(Poti3 < PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;	335	if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
337	if(Poti4 < PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;	336	if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
338	if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;	337	if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
339	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;	338	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
340	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;	339	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
341	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;	340	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
Line 386...	Line 385...
386	CHK_POTI(Parameter_Hoehe_P,ParamSet.Hoehe_P,0,100);	385	CHK_POTI(Parameter_Hoehe_P,ParamSet.Hoehe_P,0,100);
387	CHK_POTI(Parameter_Hoehe_ACC_Wirkung,ParamSet.Hoehe_ACC_Wirkung,0,255);	386	CHK_POTI(Parameter_Hoehe_ACC_Wirkung,ParamSet.Hoehe_ACC_Wirkung,0,255);
388	CHK_POTI(Parameter_KompassWirkung,ParamSet.KompassWirkung,0,255);	387	CHK_POTI(Parameter_KompassWirkung,ParamSet.KompassWirkung,0,255);
389	CHK_POTI(Parameter_Gyro_P,ParamSet.Gyro_P,10,255);	388	CHK_POTI(Parameter_Gyro_P,ParamSet.Gyro_P,10,255);
390	CHK_POTI(Parameter_Gyro_I,ParamSet.Gyro_I,0,255);	389	CHK_POTI(Parameter_Gyro_I,ParamSet.Gyro_I,0,255);
391	CHK_POTI(Parameter_I_Faktor,ParamSet.I_Faktor,0,255);	390	CHK_POTI(Parameter_I_Factor,ParamSet.I_Factor,0,255);
392	CHK_POTI(Parameter_UserParam1,ParamSet.UserParam1,0,255);	391	CHK_POTI(Parameter_UserParam1,ParamSet.UserParam1,0,255);
393	CHK_POTI(Parameter_UserParam2,ParamSet.UserParam2,0,255);	392	CHK_POTI(Parameter_UserParam2,ParamSet.UserParam2,0,255);
394	CHK_POTI(Parameter_UserParam3,ParamSet.UserParam3,0,255);	393	CHK_POTI(Parameter_UserParam3,ParamSet.UserParam3,0,255);
395	CHK_POTI(Parameter_UserParam4,ParamSet.UserParam4,0,255);	394	CHK_POTI(Parameter_UserParam4,ParamSet.UserParam4,0,255);
396	CHK_POTI(Parameter_UserParam5,ParamSet.UserParam5,0,255);	395	CHK_POTI(Parameter_UserParam5,ParamSet.UserParam5,0,255);
Line 401...	Line 400...
401	CHK_POTI(Parameter_LoopGasLimit,ParamSet.LoopGasLimit,0,255);	400	CHK_POTI(Parameter_LoopGasLimit,ParamSet.LoopGasLimit,0,255);
402	CHK_POTI(Parameter_AchsKopplung1, ParamSet.AchsKopplung1,0,255);	401	CHK_POTI(Parameter_AchsKopplung1, ParamSet.AchsKopplung1,0,255);
403	CHK_POTI(Parameter_AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255);	402	CHK_POTI(Parameter_AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255);
404	CHK_POTI(Parameter_DynamicStability,ParamSet.DynamicStability,0,255);	403	CHK_POTI(Parameter_DynamicStability,ParamSet.DynamicStability,0,255);
Line 405...	Line 404...
405		404
406	Ki = (float) Parameter_I_Faktor * 0.0001;	405	Ki = (float) Parameter_I_Factor * 0.0001;
407	MAX_GAS = ParamSet.Gas_Max;	406	MAX_GAS = ParamSet.Gas_Max;
408	MIN_GAS = ParamSet.Gas_Min;	407	MIN_GAS = ParamSet.Gas_Min;
Line 457...	Line 456...
457	Notlandung = 0;	456	Notlandung = 0;
458	}	457	}
459	ROT_ON;	458	ROT_ON;
460	if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken	459	if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken
461	{	460	{
462	GasMischanteil = ParamSet.NotGas;	461	GasMischanteil = ParamSet.EmergencyGas;
463	Notlandung = 1;	462	Notlandung = 1;
464	PPM_in[ParamSet.Kanalbelegung[K_NICK]] = 0;	463	PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0;
465	PPM_in[ParamSet.Kanalbelegung[K_ROLL]] = 0;	464	PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
466	PPM_in[ParamSet.Kanalbelegung[K_GIER]] = 0;	465	PPM_in[ParamSet.ChannelAssignment[CH_GIER]] = 0;
467	}	466	}
468	else MotorenEin = 0;	467	else MotorenEin = 0;
469	}	468	}
470	else	469	else
471	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	470	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
472	// Emfang gut	471	// Emfang gut
473	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	472	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
474	if(SenderOkay > 140)	473	if(SenderOkay > 140)
475	{	474	{
476	Notlandung = 0;	475	Notlandung = 0;
477	RcLostTimer = ParamSet.NotGasZeit * 50;	476	RcLostTimer = ParamSet.EmergencyGasDuration * 50;
478	if(GasMischanteil > 40)	477	if(GasMischanteil > 40)
479	{	478	{
480	if(modell_fliegt < 0xffff) modell_fliegt++;	479	if(modell_fliegt < 0xffff) modell_fliegt++;
481	}	480	}
482	if((modell_fliegt < 200) \|\| (GasMischanteil < 40))	481	if((modell_fliegt < 200) \|\| (GasMischanteil < 40))
Line 484...	Line 483...
484	SummeNick = 0;	483	SummeNick = 0;
485	SummeRoll = 0;	484	SummeRoll = 0;
486	Mess_Integral_Gier = 0;	485	Mess_Integral_Gier = 0;
487	Mess_Integral_Gier2 = 0;	486	Mess_Integral_Gier2 = 0;
488	}	487	}
489	if((PPM_in[ParamSet.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)	488	if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorenEin == 0)
490	{	489	{
491	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	490	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
492	// auf Nullwerte kalibrieren	491	// auf Nullwerte kalibrieren
493	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	492	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
494	if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] > 75) // Neutralwerte	493	if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] > 75) // Neutralwerte
495	{	494	{
496	if(++delay_neutral > 200) // nicht sofort	495	if(++delay_neutral > 200) // nicht sofort
497	{	496	{
498	GRN_OFF;	497	GRN_OFF;
499	MotorenEin = 0;	498	MotorenEin = 0;
500	delay_neutral = 0;	499	delay_neutral = 0;
501	modell_fliegt = 0;	500	modell_fliegt = 0;
502	if(PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70 \|\| abs(PPM_in[ParamSet.Kanalbelegung[K_ROLL]]) > 70)	501	if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70 \|\| abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70)
503	{	502	{
504	unsigned char setting=1;	503	unsigned char setting=1;
505	if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > 70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] < 70) setting = 1;	504	if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 1;
506	if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > 70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70) setting = 2;	505	if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 2;
507	if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] < 70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70) setting = 3;	506	if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 3;
508	if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] <-70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70) setting = 4;	507	if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 4;
509	if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] <-70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] < 70) setting = 5;	508	if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 5;
510	SetActiveParamSet(setting); // aktiven Datensatz merken	509	SetActiveParamSet(setting); // aktiven Datensatz merken
511	}	510	}
512	if((ParamSet.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?	511	if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Höhenregelung aktiviert?
513	{	512	{
514	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();	513	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();
515	}	514	}
516	ParamSet_ReadFromEEProm(GetActiveParamSet());	515	ParamSet_ReadFromEEProm(GetActiveParamSet());
517	SetNeutral();	516	SetNeutral();
518	Piep(GetActiveParamSet());	517	Piep(GetActiveParamSet());
519	}	518	}
520	}	519	}
521	else	520	else
522	if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern	521	if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] < -75) // ACC Neutralwerte speichern
523	{	522	{
524	if(++delay_neutral > 200) // nicht sofort	523	if(++delay_neutral > 200) // nicht sofort
525	{	524	{
526	GRN_OFF;	525	GRN_OFF;
527	SetParamByte(PID_ACC_NICK,0xFF); // Werte löschen	526	SetParamByte(PID_ACC_NICK,0xFF); // Werte löschen
Line 541...	Line 540...
541	else delay_neutral = 0;	540	else delay_neutral = 0;
542	}	541	}
543	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	542	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
544	// Gas ist unten	543	// Gas ist unten
545	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	544	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
546	if(PPM_in[ParamSet.Kanalbelegung[K_GAS]] < 35-120)	545	if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < 35-120)
547	{	546	{
548	// Starten	547	// Starten
549	if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] < -75)	548	if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] < -75)
550	{	549	{
551	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	550	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
552	// Einschalten	551	// Einschalten
553	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	552	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
554	if(++delay_einschalten > 200)	553	if(++delay_einschalten > 200)
Line 570...	Line 569...
570	else delay_einschalten = 0;	569	else delay_einschalten = 0;
571	//Auf Neutralwerte setzen	570	//Auf Neutralwerte setzen
572	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	571	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
573	// Auschalten	572	// Auschalten
574	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	573	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
575	if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] > 75)	574	if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] > 75)
576	{	575	{
577	if(++delay_ausschalten > 200) // nicht sofort	576	if(++delay_ausschalten > 200) // nicht sofort
578	{	577	{
579	MotorenEin = 0;	578	MotorenEin = 0;
580	delay_ausschalten = 200;	579	delay_ausschalten = 200;
Line 591...	Line 590...
591	if(!NewPpmData-- \|\| Notlandung)	590	if(!NewPpmData-- \|\| Notlandung)
592	{	591	{
593	int tmp_int;	592	int tmp_int;
594	static int stick_nick,stick_roll;	593	static int stick_nick,stick_roll;
595	ParameterZuordnung();	594	ParameterZuordnung();
596	StickNick = (StickNick * 3 + PPM_in[ParamSet.Kanalbelegung[K_NICK]] * ParamSet.Stick_P) / 4;	595	StickNick = (StickNick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_P) / 4;
597	StickNick += PPM_diff[ParamSet.Kanalbelegung[K_NICK]] * ParamSet.Stick_D;	596	StickNick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_D;
598	StickRoll = (StickRoll * 3 + PPM_in[ParamSet.Kanalbelegung[K_ROLL]] * ParamSet.Stick_P) / 4;	597	StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
599	StickRoll += PPM_diff[ParamSet.Kanalbelegung[K_ROLL]] * ParamSet.Stick_D;	598	StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
600		599
601	StickGier = -PPM_in[ParamSet.Kanalbelegung[K_GIER]];	600	StickGier = -PPM_in[ParamSet.ChannelAssignment[CH_GIER]];
602	StickGas = PPM_in[ParamSet.Kanalbelegung[K_GAS]] + 120;	601	StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;
603		602
604	if(abs(PPM_in[ParamSet.Kanalbelegung[K_NICK]]) > MaxStickNick)	603	if(abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]) > MaxStickNick)
605	MaxStickNick = abs(PPM_in[ParamSet.Kanalbelegung[K_NICK]]); else MaxStickNick--;	604	MaxStickNick = abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]); else MaxStickNick--;
606	if(abs(PPM_in[ParamSet.Kanalbelegung[K_ROLL]]) > MaxStickRoll)	605	if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll)
607	MaxStickRoll = abs(PPM_in[ParamSet.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;	606	MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); else MaxStickRoll--;
608	if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}	607	if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}
Line 609...	Line 608...
609		608
610	GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0;	609	GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0;
Line 646...	Line 645...
646	if(GyroFaktor < 0) GyroFaktor = 0;	645	if(GyroFaktor < 0) GyroFaktor = 0;
647	if(IntegralFaktor < 0) IntegralFaktor = 0;	646	if(IntegralFaktor < 0) IntegralFaktor = 0;
648	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	647	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
649	// Looping?	648	// Looping?
650	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	649	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
651	if((PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1;	650	if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT) Looping_Links = 1;
652	else	651	else
653	{	652	{
654	{	653	{
655	if((PPM_in[ParamSet.Kanalbelegung[K_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysterese))) Looping_Links = 0;	654	if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Links = 0;
656	}	655	}
657	}	656	}
658	if((PPM_in[ParamSet.Kanalbelegung[K_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;	657	if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Rechts = 1;
659	else	658	else
660	{	659	{
661	if(Looping_Rechts) // Hysterese	660	if(Looping_Rechts) // Hysterese
662	{	661	{
663	if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysterese)) Looping_Rechts = 0;	662	if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Rechts = 0;
664	}	663	}
665	}	664	}
Line 666...	Line 665...
666		665
667	if((PPM_in[ParamSet.Kanalbelegung[K_NICK]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;	666	if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Oben = 1;
668	else	667	else
669	{	668	{
670	if(Looping_Oben) // Hysterese	669	if(Looping_Oben) // Hysterese
671	{	670	{
672	if((PPM_in[ParamSet.Kanalbelegung[K_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysterese))) Looping_Oben = 0;	671	if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Oben = 0;
673	}	672	}
674	}	673	}
675	if((PPM_in[ParamSet.Kanalbelegung[K_NICK]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;	674	if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Unten = 1;
676	else	675	else
677	{	676	{
678	if(Looping_Unten) // Hysterese	677	if(Looping_Unten) // Hysterese
679	{	678	{
680	if(PPM_in[ParamSet.Kanalbelegung[K_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysterese)) Looping_Unten = 0;	679	if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Unten = 0;
681	}	680	}
Line 682...	Line 681...
682	}	681	}
683		682
Line 743...	Line 742...
743	if((MaxStickNick > 15) \|\| (MaxStickRoll > 15))	742	if((MaxStickNick > 15) \|\| (MaxStickRoll > 15))
744	{	743	{
745	tmp_long /= 3;	744	tmp_long /= 3;
746	tmp_long2 /= 3;	745	tmp_long2 /= 3;
747	}	746	}
748	if(abs(PPM_in[ParamSet.Kanalbelegung[K_GIER]]) > 25)	747	if(abs(PPM_in[ParamSet.ChannelAssignment[CH_GIER]]) > 25)
749	{	748	{
750	tmp_long /= 3;	749	tmp_long /= 3;
751	tmp_long2 /= 3;	750	tmp_long2 /= 3;
752	}	751	}
Line 783...	Line 782...
783	ausgleichRoll = IntegralFehlerRoll / ParamSet.GyroAccAbgleich;	782	ausgleichRoll = IntegralFehlerRoll / ParamSet.GyroAccAbgleich;
Line 784...	Line 783...
784		783
785	LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;	784	LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
Line 786...	Line 785...
786	LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;	785	LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
787		786
788	if((MaxStickNick > 15) \|\| (MaxStickRoll > 15) \|\| (abs(PPM_in[ParamSet.Kanalbelegung[K_GIER]]) > 25))	787	if((MaxStickNick > 15) \|\| (MaxStickRoll > 15) \|\| (abs(PPM_in[ParamSet.ChannelAssignment[CH_GIER]]) > 25))
789	{	788	{
790	LageKorrekturNick /= 2;	789	LageKorrekturNick /= 2;
Line 847...	Line 846...
847	LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;	846	LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
848	}	847	}
849	else last_n_n = 1;	848	else last_n_n = 1;
850	} else last_n_n = 0;	849	} else last_n_n = 0;
851	} else cnt = 0;	850	} else cnt = 0;
852	if(cnt > ParamSet.Driftkomp) cnt = ParamSet.Driftkomp;	851	if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
853	if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;	852	if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
854	if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;	853	if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
Line 855...	Line 854...
855		854
856	// Roll +++++++++++++++++++++++++++++++++++++++++++++++++	855	// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
Line 884...	Line 883...
884	} else	883	} else
885	{	884	{
886	cnt = 0;	885	cnt = 0;
887	}	886	}
Line 888...	Line 887...
888		887
889	if(cnt > ParamSet.Driftkomp) cnt = ParamSet.Driftkomp;	888	if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
890	if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;	889	if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
891	if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;	890	if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
892	DebugOut.Analog[27] = ausgleichRoll;	891	DebugOut.Analog[27] = ausgleichRoll;
893	DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);	892	DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
Line 918...	Line 917...
918	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	917	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
919	// Gieren	918	// Gieren
920	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	919	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
921	if(abs(StickGier) > 20) // war 35	920	if(abs(StickGier) > 20) // war 35
922	{	921	{
923	if(!(ParamSet.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;	922	if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) NeueKompassRichtungMerken = 1;
924	}	923	}
925	tmp_int = (long) ParamSet.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²	924	tmp_int = (long) ParamSet.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²
926	tmp_int += (ParamSet.Gier_P * StickGier) / 4;	925	tmp_int += (ParamSet.Gier_P * StickGier) / 4;
927	sollGier = tmp_int;	926	sollGier = tmp_int;
928	Mess_Integral_Gier -= tmp_int;	927	Mess_Integral_Gier -= tmp_int;
Line 930...	Line 929...
930	if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;	929	if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
Line 931...	Line 930...
931		930
932	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	931	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
933	// Kompass	932	// Kompass
934	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	933	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
935	if(KompassValue && (ParamSet.GlobalConfig & CFG_KOMPASS_AKTIV))	934	if(KompassValue && (ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE))
936	{	935	{
937	int w,v;	936	int w,v;
938	static int SignalSchlecht = 0;	937	static int SignalSchlecht = 0;
939	w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln	938	w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
Line 1026...	Line 1025...
1026	// Höhenregelung	1025	// Höhenregelung
1027	// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht	1026	// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
1028	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1027	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1029	//OCR0B = 180 - (Poti1 + 120) / 4;	1028	//OCR0B = 180 - (Poti1 + 120) / 4;
1030	//DruckOffsetSetting = OCR0B;	1029	//DruckOffsetSetting = OCR0B;
1031	if((ParamSet.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung	1030	if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Höhenregelung
1032	{	1031	{
1033	int tmp_int;	1032	int tmp_int;
1034	if(ParamSet.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert	1033	if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH) // Regler wird über Schalter gesteuert
1035	{	1034	{
1036	if(Parameter_MaxHoehe < 50)	1035	if(Parameter_MaxHoehe < 50)
1037	{	1036	{
1038	SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters	1037	SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
1039	HoehenReglerAktiv = 0;	1038	HoehenReglerAktiv = 0;

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/fc.c - Rev 688 → 690