WebSVN - FlightCtrl - Diff - Rev 1888 and 1892 - /branches/V0.84a_ACC-HH_HR



unsigned char Parameter_Gyro_D = 8;             // Wert : 0-250
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
unsigned char Parameter_Gyro_Gier_P = 150;      // Wert : 10-250
unsigned char Parameter_Gyro_Gier_I = 150;      // Wert : 10-250
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20  // MartinR: deaktivieren, da unbenutzt??
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
unsigned char Parameter_UserParam1 = 0;
unsigned char Parameter_UserParam2 = 0;
unsigned char Parameter_UserParam3 = 0;
unsigned char Parameter_UserParam4 = 0;

    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
    DebugOut.Analog[12] = Motor[0].SetPoint;
    DebugOut.Analog[13] = Motor[1].SetPoint;
    DebugOut.Analog[14] = Motor[2].SetPoint;
    DebugOut.Analog[15] = Motor[3].SetPoint;
       
        //DebugOut.Analog[16] = FromNC_Rotate_C;  // MartinR: test
        //DebugOut.Analog[17] = FromNC_Rotate_S;  // MartinR: test
        //DebugOut.Analog[18] = ControlHeading;  // MartinR: test
        //DebugOut.Analog[19] = MesswertRoll;  // MartinR: test
    DebugOut.Analog[20] = ServoNickValue;
    DebugOut.Analog[22] = Capacity.ActualCurrent;
    DebugOut.Analog[23] = Capacity.UsedCapacity;
        DebugOut.Analog[24] = SollHoehe/5;     

            tmpl4 *= Parameter_AchsKopplung2; //65
            tmpl4 /= 4096L;
            KopplungsteilNickRoll = tmpl3;
            KopplungsteilRollNick = tmpl4;
            tmpl4 -= tmpl3;
                        if(IntegralFaktor) // MartinR: nur im ACC-Mode
                        {
            ErsatzKompass += tmpl4;
            if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen
                        }
            tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
            tmpl *= Parameter_AchsKopplung1;  // 90
            tmpl /= 4096L;
            tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
            tmpl2 *= Parameter_AchsKopplung1;

// MartinR: geändert Ende

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// CareFree und freie Wahl der vorderen Richtung
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        //if(CareFree) // MartinR: so war es
        if(CareFree && IntegralFaktor) // MartinR: CareFree nur im ACC-Mode
        {
                signed int nick, roll;
                nick = stick_nick / 4;
                roll = stick_roll / 4;

 MittelIntegralNick2 += IntegralNick2;
 MittelIntegralRoll2 += IntegralRoll2;

 
// if(Looping_Nick || Looping_Roll)  // MartinR: so war es
   //if(Looping_Nick || Looping_Roll || (!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert
   if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: Looping deaktiviert
  {
    IntegralAccNick = 0;
    IntegralAccRoll = 0;
    MittelIntegralNick = 0;

    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 

Rev 1888	Rev 1892
Line 123...	Line 123...
123	unsigned char Parameter_Gyro_D = 8; // Wert : 0-250	123	unsigned char Parameter_Gyro_D = 8; // Wert : 0-250
124	unsigned char Parameter_Gyro_P = 150; // Wert : 10-250	124	unsigned char Parameter_Gyro_P = 150; // Wert : 10-250
125	unsigned char Parameter_Gyro_I = 150; // Wert : 0-250	125	unsigned char Parameter_Gyro_I = 150; // Wert : 0-250
126	unsigned char Parameter_Gyro_Gier_P = 150; // Wert : 10-250	126	unsigned char Parameter_Gyro_Gier_P = 150; // Wert : 10-250
127	unsigned char Parameter_Gyro_Gier_I = 150; // Wert : 10-250	127	unsigned char Parameter_Gyro_Gier_I = 150; // Wert : 10-250
128	unsigned char Parameter_Gier_P = 2; // Wert : 1-20	128	unsigned char Parameter_Gier_P = 2; // Wert : 1-20 // MartinR: deaktivieren, da unbenutzt??
129	unsigned char Parameter_I_Faktor = 10; // Wert : 1-20	129	unsigned char Parameter_I_Faktor = 10; // Wert : 1-20
130	unsigned char Parameter_UserParam1 = 0;	130	unsigned char Parameter_UserParam1 = 0;
131	unsigned char Parameter_UserParam2 = 0;	131	unsigned char Parameter_UserParam2 = 0;
132	unsigned char Parameter_UserParam3 = 0;	132	unsigned char Parameter_UserParam3 = 0;
133	unsigned char Parameter_UserParam4 = 0;	133	unsigned char Parameter_UserParam4 = 0;
Line 195...	Line 195...
195	DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;	195	DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
196	DebugOut.Analog[12] = Motor[0].SetPoint;	196	DebugOut.Analog[12] = Motor[0].SetPoint;
197	DebugOut.Analog[13] = Motor[1].SetPoint;	197	DebugOut.Analog[13] = Motor[1].SetPoint;
198	DebugOut.Analog[14] = Motor[2].SetPoint;	198	DebugOut.Analog[14] = Motor[2].SetPoint;
199	DebugOut.Analog[15] = Motor[3].SetPoint;	199	DebugOut.Analog[15] = Motor[3].SetPoint;
-		200
200	//DebugOut.Analog[16] = DiffNick; // MartinR: test	201	//DebugOut.Analog[16] = FromNC_Rotate_C; // MartinR: test
201	//DebugOut.Analog[17] = DiffRoll; // MartinR: test	202	//DebugOut.Analog[17] = FromNC_Rotate_S; // MartinR: test
202	//DebugOut.Analog[18] = MesswertNick; // MartinR: test	203	//DebugOut.Analog[18] = ControlHeading; // MartinR: test
203	//DebugOut.Analog[19] = MesswertRoll; // MartinR: test	204	//DebugOut.Analog[19] = MesswertRoll; // MartinR: test
204	DebugOut.Analog[20] = ServoNickValue;	205	DebugOut.Analog[20] = ServoNickValue;
205	DebugOut.Analog[22] = Capacity.ActualCurrent;	206	DebugOut.Analog[22] = Capacity.ActualCurrent;
206	DebugOut.Analog[23] = Capacity.UsedCapacity;	207	DebugOut.Analog[23] = Capacity.UsedCapacity;
207	DebugOut.Analog[24] = SollHoehe/5;	208	DebugOut.Analog[24] = SollHoehe/5;
Line 430...	Line 431...
430	tmpl4 *= Parameter_AchsKopplung2; //65	431	tmpl4 *= Parameter_AchsKopplung2; //65
431	tmpl4 /= 4096L;	432	tmpl4 /= 4096L;
432	KopplungsteilNickRoll = tmpl3;	433	KopplungsteilNickRoll = tmpl3;
433	KopplungsteilRollNick = tmpl4;	434	KopplungsteilRollNick = tmpl4;
434	tmpl4 -= tmpl3;	435	tmpl4 -= tmpl3;
-		436	if(IntegralFaktor) // MartinR: nur im ACC-Mode
-		437	{
435	ErsatzKompass += tmpl4;	438	ErsatzKompass += tmpl4;
436	if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen	439	if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen
437		440	}
438	tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;	441	tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
439	tmpl *= Parameter_AchsKopplung1; // 90	442	tmpl *= Parameter_AchsKopplung1; // 90
440	tmpl /= 4096L;	443	tmpl /= 4096L;
441	tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;	444	tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
442	tmpl2 *= Parameter_AchsKopplung1;	445	tmpl2 *= Parameter_AchsKopplung1;
Line 955...	Line 958...
955	// MartinR: geändert Ende	958	// MartinR: geändert Ende
Line 956...	Line 959...
956		959
957	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	960	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
958	// CareFree und freie Wahl der vorderen Richtung	961	// CareFree und freie Wahl der vorderen Richtung
959	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	962	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-		963	//if(CareFree) // MartinR: so war es
960	if(CareFree)	964	if(CareFree && IntegralFaktor) // MartinR: CareFree nur im ACC-Mode
961	{	965	{
962	signed int nick, roll;	966	signed int nick, roll;
963	nick = stick_nick / 4;	967	nick = stick_nick / 4;
964	roll = stick_roll / 4;	968	roll = stick_roll / 4;
Line 1091...	Line 1095...
1091	MittelIntegralNick2 += IntegralNick2;	1095	MittelIntegralNick2 += IntegralNick2;
1092	MittelIntegralRoll2 += IntegralRoll2;	1096	MittelIntegralRoll2 += IntegralRoll2;
Line 1093...	Line 1097...
1093		1097
1094	// if(Looping_Nick \|\| Looping_Roll) // MartinR: so war es	1098	// if(Looping_Nick \|\| Looping_Roll) // MartinR: so war es
1095	//if(Looping_Nick \|\| Looping_Roll \|\| (!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert	1099	//if(Looping_Nick \|\| Looping_Roll \|\| (!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert
1096	if((!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: Looping deaktiviert	1100	if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: Looping deaktiviert
1097	{	1101	{
1098	IntegralAccNick = 0;	1102	IntegralAccNick = 0;
1099	IntegralAccRoll = 0;	1103	IntegralAccRoll = 0;
1100	MittelIntegralNick = 0;	1104	MittelIntegralNick = 0;
Line 1106...	Line 1110...
1106	ZaehlMessungen = 0;	1110	ZaehlMessungen = 0;
1107	LageKorrekturNick = 0;	1111	LageKorrekturNick = 0;
1108	LageKorrekturRoll = 0;	1112	LageKorrekturRoll = 0;
1109	}	1113	}
Line 1110...	Line 1114...
1110		1114
1111	if((!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) ) // MartinR:	1115	if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) ) // MartinR:
1112	// nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH	1116	// nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH
1113	// um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!	1117	// um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!
1114	// bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet	1118	// bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet

Subversion Repositories FlightCtrl

(root)/branches/V0.84a_ACC-HH_HR_MartinR/fc.c - Rev 1888 → 1892