WebSVN - FlightCtrl - Diff - Rev 1101 and 1102 - /branches/salvo_gps/Basis



//Salvo End

 
 //Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation
long GyroKomp_Int;
long int GyroGier_Comp;
//int GyroKomp_Value; //  Der ermittelte Kompasswert aus Gyro und Magnetkompass
short int cnt_stickgier_zero =0;
int gyrogier_kompass;

//Salvo End

//Salvo 2.1.2008 Allgemeine Debugvariablen
int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7;
//Salvo End

 
 
 
 
void Piep(unsigned char Anzahl)
{
 while(Anzahl--)

 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
 CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
 CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
 CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
 
 CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
 CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
 CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
 CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
 CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);

    MittelIntegralRoll2 = 0;
    ZaehlMessungen = 0;
 }
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);

 
// Salvo Ersatzkompass  und Giergyrokompensation 30.12.2007 **********************
if ((Kompass_Neuer_Wert > 0) && (Kompass_present>0)) //nur wenn Kompass einen neuen Wert geliefert hat
{
   Kompass_Neuer_Wert = 0;
   w = (abs(Mittelwert_AccNick));
   v = (abs(Mittelwert_AccRoll));
   if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok
   {
        if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
        {
          if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass
          {
                        if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;
                        if (cnt_stickgier_zero > 1) //nur Abgleichen wenn keine Stickbewegung da
                        {
                                w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR);
                                v = KompassValue - gyrogier_kompass; //realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
                                if (v <-180) v +=360; // Uberlaufkorrektur

                                if (v > 180) v -=360; // Uberlaufkorrektur

 
 
                                v = w-v;  //Differenz Gyro zu Kompass ist der Driftfehler
 
                                if (v > GIER_COMP_MAX)  v= GIER_COMP_MAX;
                                if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;

          }
          if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360L;
          GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern
        }
   }
   else //Kompass wegen zu grosser Neigung ungueltig
   {
    magkompass_ok = 0;
        GyroGier_Comp = 0;
   }
   Kompass_Value_Old =KompassValue;

Rev 1101	Rev 1102
Line 100...	Line 100...
100	//Salvo End	100	//Salvo End
Line 101...	Line 101...
101		101
102	//Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation	102	//Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation
103	long GyroKomp_Int;	103	long GyroKomp_Int;
104	long int GyroGier_Comp;	104	long int GyroGier_Comp;
105	int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass	105	//int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass
106	short int cnt_stickgier_zero =0;	106	short int cnt_stickgier_zero =0;
107	int gyrogier_kompass;	107	int gyrogier_kompass;
Line 108...	Line 108...
108	//Salvo End	108	//Salvo End
Line 171...	Line 171...
171	//Salvo 2.1.2008 Allgemeine Debugvariablen	171	//Salvo 2.1.2008 Allgemeine Debugvariablen
172	int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7;	172	int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7;
173	//Salvo End	173	//Salvo End
Line 174...	Line -...
174		-
175		-
176		174
177		175
178	void Piep(unsigned char Anzahl)	176	void Piep(unsigned char Anzahl)
179	{	177	{
180	while(Anzahl--)	178	while(Anzahl--)
Line 486...	Line 484...
486	CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255);	484	CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255);
487	CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);	485	CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
488	CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);	486	CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
489	CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);	487	CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
490	CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);	488	CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
491		-
492	CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);	489	CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
493	CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);	490	CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
494	CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);	491	CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
495	CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);	492	CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
496	CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);	493	CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
Line 1100...	Line 1097...
1100	MittelIntegralRoll2 = 0;	1097	MittelIntegralRoll2 = 0;
1101	ZaehlMessungen = 0;	1098	ZaehlMessungen = 0;
1102	}	1099	}
1103	//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);	1100	//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
Line 1104...	Line 1101...
1104		1101
1105	// Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 **********************	1102	// Salvo Ersatzkompass und Giergyrokompensation 30.12.2007 **********************
1106	if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat	1103	if ((Kompass_Neuer_Wert > 0) && (Kompass_present>0)) //nur wenn Kompass einen neuen Wert geliefert hat
1107	{	1104	{
1108	Kompass_Neuer_Wert = 0;	1105	Kompass_Neuer_Wert = 0;
1109	w = (abs(Mittelwert_AccNick));	1106	w = (abs(Mittelwert_AccNick));
1110	v = (abs(Mittelwert_AccRoll));	1107	v = (abs(Mittelwert_AccRoll));
1111	if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT) && (Kompass_present>0)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok	1108	if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok
1112	{	1109	{
1113	if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen	1110	if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
1114	{	1111	{
1115	if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass	1112	if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass
1116	{	1113	{
1117	if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;	1114	if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;
1118	if (cnt_stickgier_zero > 10) // nur Abgleichen wenn keine Stickbewegung da	1115	if (cnt_stickgier_zero > 1) //nur Abgleichen wenn keine Stickbewegung da
1119	{	1116	{
1120	w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR);	1117	w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR);
1121	v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen	1118	v = KompassValue - gyrogier_kompass; //realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
1122	if (v <-180) v +=360; // Uberlaufkorrektur	1119	if (v <-180) v +=360; // Uberlaufkorrektur
Line 1123...	Line 1120...
1123	if (v > 180) v -=360; // Uberlaufkorrektur	1120	if (v > 180) v -=360; // Uberlaufkorrektur
Line 1124...	Line -...
1124		-
1125	v = w-v; //Differenz Gyro zu Kompass ist der Driftfehler	1121
1126		1122	v = w-v; //Differenz Gyro zu Kompass ist der Driftfehler
1127	#define GIER_COMP_MAX 4	1123
1128	if (v > GIER_COMP_MAX) v= GIER_COMP_MAX;	1124	if (v > GIER_COMP_MAX) v= GIER_COMP_MAX;
1129	if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;	1125	if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
Line 1164...	Line 1160...
1164	}	1160	}
1165	if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360L;	1161	if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360L;
1166	GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern	1162	GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern
1167	}	1163	}
1168	}	1164	}
1169	else //Kompass wegen grosser Neigung ungueltig	1165	else //Kompass wegen zu grosser Neigung ungueltig
1170	{	1166	{
1171	magkompass_ok = 0;	1167	magkompass_ok = 0;
1172	GyroGier_Comp = 0;	1168	GyroGier_Comp = 0;
1173	}	1169	}
1174	Kompass_Value_Old =KompassValue;	1170	Kompass_Value_Old =KompassValue;

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_V0071h/trunk/fc.c - Rev 1101 → 1102