WebSVN - FlightCtrl - Diff - Rev 851 and 857 - /branches/MicroMag3



long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;

 
float GyroFaktor;
float IntegralFaktor;
int DiffNick,DiffRoll;
uint8_t Poti1, Poti2, Poti3, Poti4;
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
unsigned char MotorWert[5];
volatile unsigned char SenderOkay = 0;
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 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;
uint8_t Cam_Travel;
int8_t nick_tilt, roll_tilt;

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

int MaxStickNick = 0,MaxStickRoll = 0;

 
//############################################################################
//  Nullwerte ermitteln
void SetNeutral(void)
//############################################################################
{      
        acc_neutral.X = 0;
        acc_neutral.Y = 0;
        acc_neutral.Z = 0;
    AdNeutralNick = 0; 

    MesswertGier = 0;
    StartLuftdruck = Luftdruck;
    HoeheD = 0;
    Mess_Integral_Hoch = 0;
    KompassStartwert = KompassValue;
 
    beeptime = 50;  
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
    ExternHoehenValue = 0;
        Cam_Travel = EE_Parameter.ServoNickMax - EE_Parameter.ServoNickMin;
}

 
//############################################################################
// Bearbeitet die Messwerte

             Mess_IntegralRoll =  (Umschlag180Roll - 10000L);
             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;
                         }
                        #endif
// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
            MesswertNick -= tmpl2;
            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
            Mess_IntegralNick2 += MesswertNick;
            Mess_IntegralNick  += MesswertNick - LageKorrekturNick;

             Mess_IntegralNick =  (Umschlag180Nick - 10000L);
             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;
                         }
                        #endif
//++++++++++++++++++++++++++++++++++++++++++++++++
// ADC einschalten
    ANALOG_ON; 
//++++++++++++++++++++++++++++++++++++++++++++++++

    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[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--;
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--;
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--;
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 < 255) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 > 0) Poti4--;
 
 
 
 
}

 
//############################################################################
// Messwerte beim Ermitteln der Nullage

        MesswertGier = AdWertGier;
        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
   // ADC einschalten
    ANALOG_ON; 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--;
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--;
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--;
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 < 255) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 > 0) Poti4--;
 
 
 
 
 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
}

 

                        PPM_in_gier = 0;
                }
        else MotorenEin = 0;
        }
        else
               
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
// Emfang gut
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
        if(SenderOkay > 140)
            {

                Mess_Integral_Gier = 0;
                Mess_Integral_Gier2 = 0;
                }
            if((PPM_in_gas > 80) && MotorenEin == 0)
                {
                               
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
// auf Nullwerte kalibrieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
                if(PPM_in_gier > 75)  // Neutralwerte
                    {

                        Piep(GetActiveParamSetNumber());
                        }
                    }
                 else delay_neutral = 0;
                }
                               
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
// Gas ist unten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
            if(PPM_in_gas < -85)
                {
                // Starten
                if(PPM_in_gier < -75)
                    {
                                       
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
                    if(++delay_einschalten > 200)
                        {

                        SummeRoll = 0;
                        }          
                    }  
                    else delay_einschalten = 0;
                //Auf Neutralwerte setzen
                               
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
// Auschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
                if(PPM_in_gier > 75)
                    {

   if(abs(PPM_in_roll) > MaxStickRoll)
     MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--;
   if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}

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

    IntegralFaktor = ((float)Parameter_Gyro_I) / 44000;
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Digitale Steuerung per DubWise

    if(DubWiseKeys[0] & 16) ExternHoehenValue--;

 
    StickNick += ExternStickNick / 8;
    StickRoll += ExternStickRoll / 8;
    StickGier += ExternStickGier;
       
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)

    }

 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
    if(GyroFaktor < 0) GyroFaktor = 0;
    if(IntegralFaktor < 0) IntegralFaktor = 0;
       
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
  if((PPM_in_roll > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;

     IntegralFaktor = 0.003;
     Looping_Roll = 0;
     Looping_Nick = 0;
    }  

 
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
// Integrale auf ACC-Signal abgleichen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

   if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in_gier) > 25))
    {
    tmp_long  /= 3;
    tmp_long2 /= 3;
    }
 
 
 
 
 

 
#define AUSGLEICH 32
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;

//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
//  Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
    if(abs(StickGier) > 20)
     {
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
     }
    tmp_int  = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
//  Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
        if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
        {
                if (!updKompass--)              // Aufruf mit ~15 Hz
                {
                        updKompass = 33;

                       
                        if ((MaxStickNick < 50) && (MaxStickRoll < 50))         // Bei extremen Flugmanövern keine Kompassauswertung
                        {
                                KompassValue = heading_MM3();

//  Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
        if(!TimerWerteausgabe--)
        {
        TimerWerteausgabe = 24;
 
        //DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
    //DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
    DebugOut.Analog[2] = Mittelwert_AccNick;
    DebugOut.Analog[3] = Mittelwert_AccRoll;
    DebugOut.Analog[4] = MesswertGier;
    DebugOut.Analog[5] = HoehenWert;
    DebugOut.Analog[6] = Mess_Integral_Hoch / 512;

 
    if(GasMischanteil < 20) GierMischanteil = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
// Nick-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
    else  SummeNick += DiffNick; // I-Anteil bei HH 
    if(SummeNick >  16000) SummeNick =  16000;
    if(SummeNick < -16000) SummeNick = -16000;
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                      

        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
        Motor_Hinten = motorwert;              
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
// Roll-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
    if(SummeRoll >  16000) SummeRoll =  16000;
    if(SummeRoll < -16000) SummeRoll = -16000;
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
    // Motor Links
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 

 
        if ((motorwert < 0)) motorwert = 0;
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;

Rev 851	Rev 857
Line 83...	Line 83...
83	long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;	83	long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
Line 84...	Line 84...
84		84
85	float GyroFaktor;	85	float GyroFaktor;
86	float IntegralFaktor;	86	float IntegralFaktor;
87	int DiffNick,DiffRoll;	87	int DiffNick,DiffRoll;
88	int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;	88	uint8_t Poti1, Poti2, Poti3, Poti4;
89	volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;	89	volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
90	unsigned char MotorWert[5];	90	unsigned char MotorWert[5];
91	volatile unsigned char SenderOkay = 0;	91	volatile unsigned char SenderOkay = 0;
92	int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;	92	int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
Line 118...	Line 118...
118	unsigned char Parameter_ServoNickControl = 100;	118	unsigned char Parameter_ServoNickControl = 100;
119	unsigned char Parameter_LoopGasLimit = 70;	119	unsigned char Parameter_LoopGasLimit = 70;
120	unsigned char Parameter_AchsKopplung1 = 0;	120	unsigned char Parameter_AchsKopplung1 = 0;
121	unsigned char Parameter_AchsGegenKopplung1 = 0;	121	unsigned char Parameter_AchsGegenKopplung1 = 0;
122	unsigned char Parameter_DynamicStability = 100;	122	unsigned char Parameter_DynamicStability = 100;
-		123	uint8_t Cam_Travel;
-		124	int8_t nick_tilt, roll_tilt;
Line 123...	Line 125...
123		125
124	signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;	126	signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
Line 125...	Line 127...
125	int MaxStickNick = 0,MaxStickRoll = 0;	127	int MaxStickNick = 0,MaxStickRoll = 0;
Line 173...	Line 175...
173		175
174	//############################################################################	176	//############################################################################
175	// Nullwerte ermitteln	177	// Nullwerte ermitteln
176	void SetNeutral(void)	178	void SetNeutral(void)
177	//############################################################################	179	//############################################################################
178	{	180	{
179	acc_neutral.X = 0;	181	acc_neutral.X = 0;
180	acc_neutral.Y = 0;	182	acc_neutral.Y = 0;
181	acc_neutral.Z = 0;	183	acc_neutral.Z = 0;
182	AdNeutralNick = 0;	184	AdNeutralNick = 0;
Line 210...	Line 212...
210	MesswertGier = 0;	212	MesswertGier = 0;
211	StartLuftdruck = Luftdruck;	213	StartLuftdruck = Luftdruck;
212	HoeheD = 0;	214	HoeheD = 0;
213	Mess_Integral_Hoch = 0;	215	Mess_Integral_Hoch = 0;
214	KompassStartwert = KompassValue;	216	KompassStartwert = KompassValue;
215	GPS_Neutral();	-
216	beeptime = 50;	217	beeptime = 50;
217	Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;	218	Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
218	Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;	219	Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
219	ExternHoehenValue = 0;	220	ExternHoehenValue = 0;
-		221	Cam_Travel = EE_Parameter.ServoNickMax - EE_Parameter.ServoNickMin;
220	}	222	}
Line 221...	Line 223...
221		223
222	//############################################################################	224	//############################################################################
223	// Bearbeitet die Messwerte	225	// Bearbeitet die Messwerte
Line 268...	Line 270...
268	Mess_IntegralRoll = (Umschlag180Roll - 10000L);	270	Mess_IntegralRoll = (Umschlag180Roll - 10000L);
269	Mess_IntegralRoll2 = Mess_IntegralRoll;	271	Mess_IntegralRoll2 = Mess_IntegralRoll;
270	}	272	}
271	if(AdWertRoll < 15) MesswertRoll = -1000;	273	if(AdWertRoll < 15) MesswertRoll = -1000;
272	if(AdWertRoll < 7) MesswertRoll = -2000;	274	if(AdWertRoll < 7) MesswertRoll = -2000;
273	if(PlatinenVersion == 10)	275	#if (PlatinenVersion == 10)
274	{	276	{
275	if(AdWertRoll > 1010) MesswertRoll = +1000;	277	if(AdWertRoll > 1010) MesswertRoll = +1000;
276	if(AdWertRoll > 1017) MesswertRoll = +2000;	278	if(AdWertRoll > 1017) MesswertRoll = +2000;
277	}	279	}
278	else	280	#else
279	{	281	{
280	if(AdWertRoll > 2020) MesswertRoll = +1000;	282	if(AdWertRoll > 2020) MesswertRoll = +1000;
281	if(AdWertRoll > 2034) MesswertRoll = +2000;	283	if(AdWertRoll > 2034) MesswertRoll = +2000;
282	}	284	}
-		285	#endif
283	// Nick ++++++++++++++++++++++++++++++++++++++++++++++++	286	// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
284	MesswertNick -= tmpl2;	287	MesswertNick -= tmpl2;
285	MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;	288	MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
286	Mess_IntegralNick2 += MesswertNick;	289	Mess_IntegralNick2 += MesswertNick;
287	Mess_IntegralNick += MesswertNick - LageKorrekturNick;	290	Mess_IntegralNick += MesswertNick - LageKorrekturNick;
Line 295...	Line 298...
295	Mess_IntegralNick = (Umschlag180Nick - 10000L);	298	Mess_IntegralNick = (Umschlag180Nick - 10000L);
296	Mess_IntegralNick2 = Mess_IntegralNick;	299	Mess_IntegralNick2 = Mess_IntegralNick;
297	}	300	}
298	if(AdWertNick < 15) MesswertNick = -1000;	301	if(AdWertNick < 15) MesswertNick = -1000;
299	if(AdWertNick < 7) MesswertNick = -2000;	302	if(AdWertNick < 7) MesswertNick = -2000;
300	if(PlatinenVersion == 10)	303	#if(PlatinenVersion == 10)
301	{	304	{
302	if(AdWertNick > 1010) MesswertNick = +1000;	305	if(AdWertNick > 1010) MesswertNick = +1000;
303	if(AdWertNick > 1017) MesswertNick = +2000;	306	if(AdWertNick > 1017) MesswertNick = +2000;
304	}	307	}
305	else	308	#else
306	{	309	{
307	if(AdWertNick > 2020) MesswertNick = +1000;	310	if(AdWertNick > 2020) MesswertNick = +1000;
308	if(AdWertNick > 2034) MesswertNick = +2000;	311	if(AdWertNick > 2034) MesswertNick = +2000;
309	}	312	}
-		313	#endif
310	//++++++++++++++++++++++++++++++++++++++++++++++++	314	//++++++++++++++++++++++++++++++++++++++++++++++++
311	// ADC einschalten	315	// ADC einschalten
312	ANALOG_ON;	316	ANALOG_ON;
313	//++++++++++++++++++++++++++++++++++++++++++++++++	317	//++++++++++++++++++++++++++++++++++++++++++++++++
Line 323...	Line 327...
323	if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);	327	if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);
324	else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);	328	else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
325	if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200);	329	if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200);
326	else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);	330	else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
327	}	331	}
-		332
328	if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;	333	if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--;
329	if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;	334	if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--;
330	if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;	335	if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--;
331	if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;	336	if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 < 255) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 > 0) Poti4--;
332	if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;	-
333	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;	-
334	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;	-
335	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;	-
336	}	337	}
Line 337...	Line 338...
337		338
338	//############################################################################	339	//############################################################################
339	// Messwerte beim Ermitteln der Nullage	340	// Messwerte beim Ermitteln der Nullage
Line 347...	Line 348...
347	MesswertGier = AdWertGier;	348	MesswertGier = AdWertGier;
348	Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;	349	Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
349	Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;	350	Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
350	// ADC einschalten	351	// ADC einschalten
351	ANALOG_ON;	352	ANALOG_ON;
352	if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;	353	if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--;
353	if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;	354	if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--;
354	if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;	355	if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--;
355	if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;	356	if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 < 255) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 > 0) Poti4--;
356	if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;	-
357	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;	-
358	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;	-
359	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;	-
360		-
361	Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;	357	Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
362	Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;	358	Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
363	}	359	}
Line 364...	Line 360...
364		360
Line 497...	Line 493...
497	PPM_in_gier = 0;	493	PPM_in_gier = 0;
498	}	494	}
499	else MotorenEin = 0;	495	else MotorenEin = 0;
500	}	496	}
501	else	497	else
-		498
502	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	499	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
503	// Emfang gut	500	// Emfang gut
504	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	501	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
505	if(SenderOkay > 140)	502	if(SenderOkay > 140)
506	{	503	{
Line 517...	Line 514...
517	Mess_Integral_Gier = 0;	514	Mess_Integral_Gier = 0;
518	Mess_Integral_Gier2 = 0;	515	Mess_Integral_Gier2 = 0;
519	}	516	}
520	if((PPM_in_gas > 80) && MotorenEin == 0)	517	if((PPM_in_gas > 80) && MotorenEin == 0)
521	{	518	{
-		519
522	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	520	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
523	// auf Nullwerte kalibrieren	521	// auf Nullwerte kalibrieren
524	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	522	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
525	if(PPM_in_gier > 75) // Neutralwerte	523	if(PPM_in_gier > 75) // Neutralwerte
526	{	524	{
Line 545...	Line 543...
545	Piep(GetActiveParamSetNumber());	543	Piep(GetActiveParamSetNumber());
546	}	544	}
547	}	545	}
548	else delay_neutral = 0;	546	else delay_neutral = 0;
549	}	547	}
-		548
550	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	549	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
551	// Gas ist unten	550	// Gas ist unten
552	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	551	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
553	if(PPM_in_gas < 35-120)	552	if(PPM_in_gas < -85)
554	{	553	{
555	// Starten	554	// Starten
556	if(PPM_in_gier < -75)	555	if(PPM_in_gier < -75)
557	{	556	{
-		557
558	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	558	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
559	// Einschalten	559	// Einschalten
560	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	560	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
561	if(++delay_einschalten > 200)	561	if(++delay_einschalten > 200)
562	{	562	{
Line 574...	Line 574...
574	SummeRoll = 0;	574	SummeRoll = 0;
575	}	575	}
576	}	576	}
577	else delay_einschalten = 0;	577	else delay_einschalten = 0;
578	//Auf Neutralwerte setzen	578	//Auf Neutralwerte setzen
-		579
579	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	580	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
580	// Auschalten	581	// Auschalten
581	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	582	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
582	if(PPM_in_gier > 75)	583	if(PPM_in_gier > 75)
583	{	584	{
Line 612...	Line 613...
612	if(abs(PPM_in_roll) > MaxStickRoll)	613	if(abs(PPM_in_roll) > MaxStickRoll)
613	MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--;	614	MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--;
614	if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}	615	if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}
Line 615...	Line 616...
615		616
616	GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0;	617	GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0;
Line 617...	Line 618...
617	IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;	618	IntegralFaktor = ((float)Parameter_Gyro_I) / 44000;
618		619
619	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	620	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
620	//+ Digitale Steuerung per DubWise	621	//+ Digitale Steuerung per DubWise
Line 634...	Line 635...
634	if(DubWiseKeys[0] & 16) ExternHoehenValue--;	635	if(DubWiseKeys[0] & 16) ExternHoehenValue--;
Line 635...	Line 636...
635		636
636	StickNick += ExternStickNick / 8;	637	StickNick += ExternStickNick / 8;
637	StickRoll += ExternStickRoll / 8;	638	StickRoll += ExternStickRoll / 8;
-		639	StickGier += ExternStickGier;
638	StickGier += ExternStickGier;	640
639	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	641	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
640	//+ Analoge Steuerung per Seriell	642	//+ Analoge Steuerung per Seriell
641	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	643	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
642	if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)	644	if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)
Line 649...	Line 651...
649	}	651	}
Line 650...	Line 652...
650		652
651	if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;	653	if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
652	if(GyroFaktor < 0) GyroFaktor = 0;	654	if(GyroFaktor < 0) GyroFaktor = 0;
-		655	if(IntegralFaktor < 0) IntegralFaktor = 0;
653	if(IntegralFaktor < 0) IntegralFaktor = 0;	656
654	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	657	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
655	// Looping?	658	// Looping?
656	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	659	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
657	if((PPM_in_roll > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1;	660	if((PPM_in_roll > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1;
Line 710...	Line 713...
710	IntegralFaktor = 0.003;	713	IntegralFaktor = 0.003;
711	Looping_Roll = 0;	714	Looping_Roll = 0;
712	Looping_Nick = 0;	715	Looping_Nick = 0;
713	}	716	}
Line 714...	Line -...
714		-
715		717
716	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	718	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
717	// Integrale auf ACC-Signal abgleichen	719	// Integrale auf ACC-Signal abgleichen
718	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	720	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 749...	Line 751...
749	if((MaxStickNick > 15) \|\| (MaxStickRoll > 15) \|\| (abs(PPM_in_gier) > 25))	751	if((MaxStickNick > 15) \|\| (MaxStickRoll > 15) \|\| (abs(PPM_in_gier) > 25))
750	{	752	{
751	tmp_long /= 3;	753	tmp_long /= 3;
752	tmp_long2 /= 3;	754	tmp_long2 /= 3;
753	}	755	}
754	//if(abs(PPM_in_gier) > 25)	-
755	// {	-
756	// tmp_long /= 3;	-
757	// tmp_long2 /= 3;	-
758	// }	-
Line 759...	Line 756...
759		756
760	#define AUSGLEICH 32	757	#define AUSGLEICH 32
761	if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;	758	if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;
762	if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH;	759	if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH;
763	if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH;	760	if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH;
Line 921...	Line 918...
921	//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);	918	//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
Line 922...	Line 919...
922		919
923	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	920	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
924	// Gieren	921	// Gieren
925	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	922	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
926	if(abs(StickGier) > 20) // war 35	923	if(abs(StickGier) > 20)
927	{	924	{
928	if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;	925	if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
929	}	926	}
930	tmp_int = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²	927	tmp_int = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²
Line 937...	Line 934...
937	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	934	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
938	// Kompass	935	// Kompass
939	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	936	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
940	if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)	937	if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
941	{	938	{
942	if (!updKompass--) // Aufruf mit ~10 Hz	939	if (!updKompass--) // Aufruf mit ~15 Hz
943	{	940	{
944	updKompass = 49;	941	updKompass = 33;
Line 945...	Line 942...
945		942
946	if ((MaxStickNick < 50) && (MaxStickRoll < 50)) // Bei extremen Flugmanövern keine Kompassauswertung	943	if ((MaxStickNick < 50) && (MaxStickRoll < 50)) // Bei extremen Flugmanövern keine Kompassauswertung
947	{	944	{
948	KompassValue = heading_MM3();	945	KompassValue = heading_MM3();
Line 961...	Line 958...
961	// Debugwerte zuordnen	958	// Debugwerte zuordnen
962	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	959	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
963	if(!TimerWerteausgabe--)	960	if(!TimerWerteausgabe--)
964	{	961	{
965	TimerWerteausgabe = 24;	962	TimerWerteausgabe = 24;
966		-
967	DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;	963	//DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
968	DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;	964	//DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
969	DebugOut.Analog[2] = Mittelwert_AccNick;	965	DebugOut.Analog[2] = Mittelwert_AccNick;
970	DebugOut.Analog[3] = Mittelwert_AccRoll;	966	DebugOut.Analog[3] = Mittelwert_AccRoll;
971	DebugOut.Analog[4] = MesswertGier;	967	DebugOut.Analog[4] = MesswertGier;
972	DebugOut.Analog[5] = HoehenWert;	968	DebugOut.Analog[5] = HoehenWert;
973	DebugOut.Analog[6] = Mess_Integral_Hoch / 512;	969	DebugOut.Analog[6] = Mess_Integral_Hoch / 512;
Line 1091...	Line 1087...
1091		1087
1092	if(GasMischanteil < 20) GierMischanteil = 0;	1088	if(GasMischanteil < 20) GierMischanteil = 0;
1093	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1089	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1094	// Nick-Achse	1090	// Nick-Achse
1095	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1091	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1096	DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen	1092	DiffNick = MesswertNick - StickNick; // Differenz bestimmen
1097	if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung	1093	if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
1098	else SummeNick += DiffNick; // I-Anteil bei HH	1094	else SummeNick += DiffNick; // I-Anteil bei HH
1099	if(SummeNick > 16000) SummeNick = 16000;	1095	if(SummeNick > 16000) SummeNick = 16000;
1100	if(SummeNick < -16000) SummeNick = -16000;	1096	if(SummeNick < -16000) SummeNick = -16000;
1101	pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick	1097	pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
Line 1116...	Line 1112...
1116	if (motorwert < MIN_GAS) motorwert = MIN_GAS;	1112	if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1117	Motor_Hinten = motorwert;	1113	Motor_Hinten = motorwert;
1118	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1114	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1119	// Roll-Achse	1115	// Roll-Achse
1120	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1116	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1121	DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen	1117	DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
1122	if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung	1118	if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung
1123	else SummeRoll += DiffRoll; // I-Anteil bei HH	1119	else SummeRoll += DiffRoll; // I-Anteil bei HH
1124	if(SummeRoll > 16000) SummeRoll = 16000;	1120	if(SummeRoll > 16000) SummeRoll = 16000;
1125	if(SummeRoll < -16000) SummeRoll = -16000;	1121	if(SummeRoll < -16000) SummeRoll = -16000;
1126	pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll	1122	pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll
1127	tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;	1123	tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1128	if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;	1124	if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
1129	if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;	1125	if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
1130	// Motor Links	1126	// Motor Links
1131	motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;	1127	motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
1132	#define GRENZE Poti1	-
Line 1133...	Line 1128...
1133		1128
1134	if ((motorwert < 0)) motorwert = 0;	1129	if ((motorwert < 0)) motorwert = 0;
1135	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;	1130	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
1136	if (motorwert < MIN_GAS) motorwert = MIN_GAS;	1131	if (motorwert < MIN_GAS) motorwert = MIN_GAS;

Subversion Repositories FlightCtrl

(root)/branches/MicroMag3_Nick666/trunc/fc.c - Rev 851 → 857