WebSVN - FlightCtrl - Diff - Rev 1747 and 1757 - /branches/V0.80g_ACC-HH_MartinR



   Mess_Integral_Gier += MesswertGier;
   ErsatzKompass += MesswertGier;
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++

         

         
          // if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 50)) IntegralFaktor = 0;  // MartinR: zusätzlich, wieder deaktiviert wegen Kameraausgleich
         

      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))  // MartinR : so war es
          //if(!Looping_Nick && !Looping_Roll && IntegralFaktor && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR: zusätzlich "&& IntegralFaktor"
          // MartinR: wieder deaktiviert wegen Kamera-Ausgleich im HH
         

//############################################################################
{
         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
         int GierMischanteil,GasMischanteil;

         

         static long SummeNickHH=0,SummeRollHH=0;
         
     static long sollGier = 0,tmp_long,tmp_long2;
     static long IntegralFehlerNick = 0;

                //stick_nick_neutral = stick_nick; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
                //stick_roll_neutral = stick_roll; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
                stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
                stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;

               
                //StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam  
                //StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam
                }
        /*else          // wenn HH , MartinR
                {
                //stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
                //stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
                //StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam
                //StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam
                }
        */

     ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
     if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
    }
    if(StickGas < 0) StickGas = 0;

 

        //if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0; // MartinR: so war es, verschoben um code zu sparen 
 
    if(abs(StickNick/STICK_GAIN) > MaxStickNick)
     {

    ZaehlMessungen = 0;
    LageKorrekturNick = 0;
    LageKorrekturRoll = 0;
  }

 
  if((!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))    ) // MartinR:
          // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH 
          // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!
          // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet 

 
        if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // MartinR geändert und verschoben

          else IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) // MartinR: so war es, wegen Kameraausgleich wieder aktiviert
  //if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin) && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt 
 

                                // ------------------------- D-Part 1: Vario Meter ----------------------------
                                tmp_int = VarioMeter / 8;
                                LIMIT_MIN_MAX(tmp_int, -127, 128);     
                                tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter
                                LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
                                /* // MartinR: so war es Anfang
                                if(HeightTrimmingFlag)  tmp_int /= 4; // reduce d-part while trimming setpoint
                                else
                                if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode
                                */ // MartinR: so war es Anfang
                                tmp_int /= 4; // MartinR: geändert
                                GasReduction += tmp_int;
                        } // EOF no baro range expanding
                        // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
            if(Parameter_Hoehe_ACC_Wirkung)
                         {

            if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)
                        {  // old version
                                LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
                                GasMischanteil = FilterHCGas;
                        }
                        //else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode // MartinR: so war es
                        else GasMischanteil = FilterHCGas ; // MartinR: geändert, um  Überschwinger bei Höhenänderung zu verringern
                  }
                }// EOF height control active
                else // HC not active
                {
                        //update hoover gas stick value when HC is not active

Rev 1747	Rev 1757
Line 409...	Line 409...
409	Mess_Integral_Gier += MesswertGier;	409	Mess_Integral_Gier += MesswertGier;
410	ErsatzKompass += MesswertGier;	410	ErsatzKompass += MesswertGier;
411	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++	411	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
Line 412...	Line 412...
412		412
Line 413...	Line 413...
413		413
414	// if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) \|\| (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // MartinR: zusätzlich	414	// if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) \|\| (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // MartinR: zusätzlich, wieder deaktiviert wegen Kameraausgleich
415		415
Line 416...	Line 416...
416	if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR : so war es	416	if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR : so war es
417	//if(!Looping_Nick && !Looping_Roll && IntegralFaktor && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR: zusätzlich "&& IntegralFaktor"	417	//if(!Looping_Nick && !Looping_Roll && IntegralFaktor && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR: zusätzlich "&& IntegralFaktor"
418	// MartinR: Test Kamera-Ausgleich im HH	418	// MartinR: wieder deaktiviert wegen Kamera-Ausgleich im HH
419		419
Line 685...	Line 685...
685	//############################################################################	685	//############################################################################
686	{	686	{
687	int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;	687	int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
688	int GierMischanteil,GasMischanteil;	688	int GierMischanteil,GasMischanteil;
Line 689...	Line 689...
689		689
Line 690...	Line 690...
690	static long SummeNickHH=0,SummeRollHH=0; // MartinR: Für ACC-HH Umschaltung Prüfen !!!!!!!!!!!!!	690	static long SummeNickHH=0,SummeRollHH=0;
691		691
692	static long sollGier = 0,tmp_long,tmp_long2;	692	static long sollGier = 0,tmp_long,tmp_long2;
693	static long IntegralFehlerNick = 0;	693	static long IntegralFehlerNick = 0;
Line 914...	Line 914...
914	//stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR	914	//stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
915	//stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR	915	//stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
916	stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;	916	stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
917	stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;	917	stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
Line 918...	Line 918...
918		918
919	//StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam wohin verschoben ?? Prüfen !!!!!!!!!!!!!!!!!!!!	919	//StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam
920	//StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam	920	//StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam
921	}	921	}
922	/*else // wenn HH , MartinR	922	/*else // wenn HH , MartinR
923	{	923	{
924	//stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben	924	//stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
925	//stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben	925	//stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
926	//StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam wohin verschoben ?? Prüfen !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!	926	//StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam
927	//StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam	927	//StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam
928	}	928	}
929	*/	929	*/
Line 980...	Line 980...
980	ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;	980	ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
981	if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;	981	if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
982	}	982	}
983	if(StickGas < 0) StickGas = 0;	983	if(StickGas < 0) StickGas = 0;
Line 984...	Line 984...
984		984
Line 985...	Line 985...
985	//if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; // MartinR: so war es, verschoben	985	//if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; // MartinR: so war es, verschoben um code zu sparen
986		986
987	if(abs(StickNick/STICK_GAIN) > MaxStickNick)	987	if(abs(StickNick/STICK_GAIN) > MaxStickNick)
988	{	988	{
Line 1086...	Line 1086...
1086	ZaehlMessungen = 0;	1086	ZaehlMessungen = 0;
1087	LageKorrekturNick = 0;	1087	LageKorrekturNick = 0;
1088	LageKorrekturRoll = 0;	1088	LageKorrekturRoll = 0;
1089	}	1089	}
Line 1090...	Line 1090...
1090		1090
1091	if(!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) // MartinR:	1091	if((!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) ) // MartinR:
1092	// nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH	1092	// nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH
1093	// um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!	1093	// um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!
1094	// bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet	1094	// bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet
Line 1105...	Line 1105...
1105		1105
1106	if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) \|\| (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // MartinR geändert und verschoben	1106	if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) \|\| (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // MartinR geändert und verschoben
Line 1107...	Line 1107...
1107	else IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen	1107	else IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen
1108		1108
1109	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1109	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 1110...	Line 1110...
1110	if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 \|\| MotorenEin)) // MartinR: so war es, für Test wieder aktiviert	1110	if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 \|\| MotorenEin)) // MartinR: so war es, wegen Kameraausgleich wieder aktiviert
1111	//if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 \|\| MotorenEin) && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt	1111	//if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 \|\| MotorenEin) && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt
1112		1112
Line 1645...	Line 1645...
1645	// ------------------------- D-Part 1: Vario Meter ----------------------------	1645	// ------------------------- D-Part 1: Vario Meter ----------------------------
1646	tmp_int = VarioMeter / 8;	1646	tmp_int = VarioMeter / 8;
1647	LIMIT_MIN_MAX(tmp_int, -127, 128);	1647	LIMIT_MIN_MAX(tmp_int, -127, 128);
1648	tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter	1648	tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter
1649	LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);	1649	LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
-		1650	/* // MartinR: so war es Anfang
1650	if(HeightTrimmingFlag) tmp_int /= 4; // reduce d-part while trimming setpoint	1651	if(HeightTrimmingFlag) tmp_int /= 4; // reduce d-part while trimming setpoint
1651	else	1652	else
1652	if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode	1653	if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode
-		1654	*/ // MartinR: so war es Anfang
-		1655	tmp_int /= 4; // MartinR: geändert
1653	GasReduction += tmp_int;	1656	GasReduction += tmp_int;
1654	} // EOF no baro range expanding	1657	} // EOF no baro range expanding
1655	// ------------------------ D-Part 2: ACC-Z Integral ------------------------	1658	// ------------------------ D-Part 2: ACC-Z Integral ------------------------
1656	if(Parameter_Hoehe_ACC_Wirkung)	1659	if(Parameter_Hoehe_ACC_Wirkung)
1657	{	1660	{
Line 1705...	Line 1708...
1705	if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)	1708	if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)
1706	{ // old version	1709	{ // old version
1707	LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas	1710	LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
1708	GasMischanteil = FilterHCGas;	1711	GasMischanteil = FilterHCGas;
1709	}	1712	}
1710	else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode	1713	//else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode // MartinR: so war es
-		1714	else GasMischanteil = FilterHCGas ; // MartinR: geändert, um Überschwinger bei Höhenänderung zu verringern
1711	}	1715	}
1712	}// EOF height control active	1716	}// EOF height control active
1713	else // HC not active	1717	else // HC not active
1714	{	1718	{
1715	//update hoover gas stick value when HC is not active	1719	//update hoover gas stick value when HC is not active

Subversion Repositories FlightCtrl

(root)/branches/V0.80g_ACC-HH_MartinR_HR/fc.c - Rev 1747 → 1757