WebSVN - FlightCtrl - Diff - Rev 173 and 188 - /branches/V0.64



    Mess_IntegralNick -= tmp_long;
    Mess_IntegralRoll -= tmp_long2;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
//  Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
    if(abs(StickGier) > 20) // war 35 
     {
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
     }
    tmp_int  = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo  y = ax + bx²
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;

//  Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
     {
       int w,v;
       static int SignalSchlecht = 0;
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
       v = abs(IntegralRoll /512);
       if(v > w) w = v; // grösste Neigung ermitteln
       if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)    
        {

  DebugOut.Analog[7] = GasMischanteil;

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
// Gier-Anteil
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
#define MUL_G  1.0
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
//GierMischanteil = 0;
    if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
    if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
    if(GierMischanteil > 100)  GierMischanteil = 100;

    if(GierMischanteil < -100) GierMischanteil = -100;
 
    if(GasMischanteil < 20) GierMischanteil = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// Nick-Achse

    if(SummeNick >  16000) SummeNick =  16000;
    if(SummeNick < -16000) SummeNick = -16000;
    pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick                                   
    // Motor Vorn
#define MUL  2
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));

 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
        if ((motorwert < 0)) motorwert = 0;
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;

        SummeRoll += DiffRoll;                                                              // I-Anteil
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
    if(SummeRoll >  16000) SummeRoll =  16000;
    if(SummeRoll < -16000) SummeRoll = -16000;
    pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
    // 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 173	Rev 188
Line 695...	Line 695...
695	Mess_IntegralNick -= tmp_long;	695	Mess_IntegralNick -= tmp_long;
696	Mess_IntegralRoll -= tmp_long2;	696	Mess_IntegralRoll -= tmp_long2;
697	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	697	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
698	// Gieren	698	// Gieren
699	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	699	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
700	if(abs(StickGier) > 35)	700	if(abs(StickGier) > 20) // war 35
701	{	701	{
702	if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;	702	if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
703	}	703	}
704	tmp_int = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo y = ax + bx²	704	tmp_int = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo y = ax + bx²
705	tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;	705	tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
Line 714...	Line 714...
714	// Kompass	714	// Kompass
715	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	715	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
716	if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))	716	if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
717	{	717	{
718	int w,v;	718	int w,v;
719	static int SignalSchlecht = 0;	719	static int SignalSchlecht = 0;
720	w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln	720	w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
721	v = abs(IntegralRoll /512);	721	v = abs(IntegralRoll /512);
722	if(v > w) w = v; // grösste Neigung ermitteln	722	if(v > w) w = v; // grösste Neigung ermitteln
723	if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)	723	if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)
724	{	724	{
Line 847...	Line 847...
847	DebugOut.Analog[7] = GasMischanteil;	847	DebugOut.Analog[7] = GasMischanteil;
Line 848...	Line 848...
848		848
849	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	849	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
850	// Gier-Anteil	850	// Gier-Anteil
851	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	851	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
852	#define MUL_G 1.4	852	#define MUL_G 1.0
853	GierMischanteil = MesswertGier - sollGier; // Regler für Gier	853	GierMischanteil = MesswertGier - sollGier; // Regler für Gier
854	//GierMischanteil = 0;	854	//GierMischanteil = 0;
855	if(GierMischanteil > (MUL_G * GasMischanteil)) GierMischanteil = MUL_G * GasMischanteil;	855	if(GierMischanteil > (MUL_G * GasMischanteil)) GierMischanteil = MUL_G * GasMischanteil;
-		856	if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
-		857	if(GierMischanteil > 100) GierMischanteil = 100;
Line 856...	Line 858...
856	if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);	858	if(GierMischanteil < -100) GierMischanteil = -100;
857		859
858	if(GasMischanteil < 10) GierMischanteil = 0;	860	if(GasMischanteil < 20) GierMischanteil = 0;
859	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	861	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 860...	Line 862...
860	// Nick-Achse	862	// Nick-Achse
Line 866...	Line 868...
866	if(SummeNick > 16000) SummeNick = 16000;	868	if(SummeNick > 16000) SummeNick = 16000;
867	if(SummeNick < -16000) SummeNick = -16000;	869	if(SummeNick < -16000) SummeNick = -16000;
868	pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick	870	pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick
869	// Motor Vorn	871	// Motor Vorn
870	#define MUL 2	872	#define MUL 2
871	if((pd_ergebnis > MUL * (GasMischanteil))) pd_ergebnis = MUL * (GasMischanteil);	873	if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil));
872	if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);	874	if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
Line 873...	Line 875...
873		875
874	motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer	876	motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer
875	if ((motorwert < 0)) motorwert = 0;	877	if ((motorwert < 0)) motorwert = 0;
876	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;	878	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
Line 889...	Line 891...
889	SummeRoll += DiffRoll; // I-Anteil	891	SummeRoll += DiffRoll; // I-Anteil
890	if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;	892	if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
891	if(SummeRoll > 16000) SummeRoll = 16000;	893	if(SummeRoll > 16000) SummeRoll = 16000;
892	if(SummeRoll < -16000) SummeRoll = -16000;	894	if(SummeRoll < -16000) SummeRoll = -16000;
893	pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll	895	pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll
894	if((pd_ergebnis > MUL * (GasMischanteil))) pd_ergebnis = MUL * (GasMischanteil);	896	if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil));
895	if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);	897	if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
896	// Motor Links	898	// Motor Links
897	motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;	899	motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
898	if ((motorwert < 0)) motorwert = 0;	900	if ((motorwert < 0)) motorwert = 0;
899	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;	901	else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
900	if (motorwert < MIN_GAS) motorwert = MIN_GAS;	902	if (motorwert < MIN_GAS) motorwert = MIN_GAS;

Subversion Repositories FlightCtrl

(root)/branches/V0.64_ZeroWarnings/fc.c @ 2751 - Rev 173 → 188