WebSVN - FlightCtrl - Diff - Rev 1662 and 1664 - /branches/V0.80g_ACC-HH_MartinR



int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;
unsigned int NeutralAccX=0, NeutralAccY=0;
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
int NeutralAccZ = 0;
unsigned char ControlHeading = 0;
long IntegralNick = 0,IntegralNick2 = 0;
long IntegralRoll = 0,IntegralRoll2 = 0;
long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
long Integral_Gier = 0;
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

 
 if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG))
  {
        static int stick_nick,stick_roll;
/*
unsigned char angle = 180/15;
signed char sintab[31] = { 0, 4, 8, 11, 14, 16, 16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16, 16};
//signed char costab[24] = {16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16};

angle = (ControlHeading + (360/15) - ErsatzKompass / (GIER_GRAD_FAKTOR * 15)) % 24;
 
signed int cos_h, sin_h;
cos_h = sintab[angle + 6]/2;
sin_h = sintab[angle]/2;
*/
    ParameterZuordnung();
    stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;

    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
 
DebugOut.Analog[16] = stick_roll;
 
if(HeadFree)
 {
    signed int nick, roll;
        nick = stick_nick / 4;
        roll = stick_roll / 4;
    StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4);
    StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4);
 }
 else
 {
    StickNick = stick_nick;

    StickRoll = stick_roll;
 }
DebugOut.Analog[17] = StickRoll;

    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//DebugOut.Analog[18] = KompassSignalSchlecht;
 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
     {
       int w,v,r,fehler,korrektur;
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
       v = abs(IntegralRoll /512);
       if(v > w) w = v; // grösste Neigung ermitteln
       korrektur = w / 8 + 2;

       fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
           //fehler += MesswertGier / 12;
 
       if(!KompassSignalSchlecht && w < 25)

                  ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
          KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
          NeueKompassRichtungMerken = 0;
         }
        }
//DebugOut.Analog[16] = fehler;
//DebugOut.Analog[17] = korrektur;
       ErsatzKompass += (fehler * 16) / korrektur;
       w = (w * Parameter_KompassWirkung) / 32;           // auf die Wirkung normieren
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
       if(w >= 0)
        {

Rev 1662	Rev 1664
Line 64...	Line 64...
64	int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;	64	int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
65	int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;	65	int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;
66	unsigned int NeutralAccX=0, NeutralAccY=0;	66	unsigned int NeutralAccX=0, NeutralAccY=0;
67	int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;	67	int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
68	int NeutralAccZ = 0;	68	int NeutralAccZ = 0;
69	unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0 , ControlHeading = 0;	69	unsigned char ControlHeading = 0;
70	long IntegralNick = 0,IntegralNick2 = 0;	70	long IntegralNick = 0,IntegralNick2 = 0;
71	long IntegralRoll = 0,IntegralRoll2 = 0;	71	long IntegralRoll = 0,IntegralRoll2 = 0;
72	long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;	72	long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
73	long Integral_Gier = 0;	73	long Integral_Gier = 0;
74	long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;	74	long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
Line 800...	Line 800...
800	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	800	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 801...	Line 801...
801		801
802	if(!NewPpmData-- \|\| (FCFlags & FCFLAG_NOTLANDUNG))	802	if(!NewPpmData-- \|\| (FCFlags & FCFLAG_NOTLANDUNG))
803	{	803	{
804	static int stick_nick,stick_roll;	804	static int stick_nick,stick_roll;
805		805	/*
806	unsigned char angle = 180/15;	806	unsigned char angle = 180/15;
807	signed char sintab[31] = { 0, 4, 8, 11, 14, 16, 16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16, 16};	807	signed char sintab[31] = { 0, 4, 8, 11, 14, 16, 16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16, 16};
808	//signed char costab[24] = {16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16};	808	//signed char costab[24] = {16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16};
Line 809...	Line 809...
809	angle = (ControlHeading + (360/15) - ErsatzKompass / (GIER_GRAD_FAKTOR * 15)) % 24;	809	angle = (ControlHeading + (360/15) - ErsatzKompass / (GIER_GRAD_FAKTOR * 15)) % 24;
810		810
811	signed int cos_h, sin_h;	811	signed int cos_h, sin_h;
812	cos_h = sintab[angle + 6]/2;	812	cos_h = sintab[angle + 6]/2;
813	sin_h = sintab[angle]/2;	813	sin_h = sintab[angle]/2;
814		814	*/
815	ParameterZuordnung();	815	ParameterZuordnung();
816	stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;	816	stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
817	stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;	817	stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
Line -...	Line 818...
-		818	stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
-		819	stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
818	stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;	820
819	stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;	821	DebugOut.Analog[16] = stick_roll;
-		822
-		823	if(HeadFree)
-		824	{
820		825	signed int nick, roll;
821	if(HeadFree)	826	nick = stick_nick / 4;
822	{	827	roll = stick_roll / 4;
823	StickNick = ((cos_h * stick_nick) + (sin_h * stick_roll))/8;	828	StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4);
824	StickRoll = ((cos_h * stick_roll) - (sin_h * stick_nick))/8;	829	StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4);
825	}	830	}
826	else	831	else
827	{	832	{
-		833	StickNick = stick_nick;
Line 828...	Line 834...
828	StickNick = stick_nick;	834	StickRoll = stick_roll;
829	StickRoll = stick_roll;	835	}
830	}	836	DebugOut.Analog[17] = StickRoll;
Line 1189...	Line 1195...
1189	if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;	1195	if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
Line 1190...	Line 1196...
1190		1196
1191	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1197	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1192	// Kompass	1198	// Kompass
1193	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1199	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 1194...	Line 1200...
1194	DebugOut.Analog[18] = KompassSignalSchlecht;	1200	//DebugOut.Analog[18] = KompassSignalSchlecht;
1195		1201
1196	if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))	1202	if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
1197	{	1203	{
1198	int w,v,r,fehler,korrektur;	1204	int w,v,r,fehler,korrektur;
1199	w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln	1205	w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
1200	v = abs(IntegralRoll /512);	1206	v = abs(IntegralRoll /512);
1201	if(v > w) w = v; // grösste Neigung ermitteln	1207	if(v > w) w = v; // grösste Neigung ermitteln
1202	korrektur = w / 8 + 2;	1208	korrektur = w / 8 + 2;
Line 1203...	Line 1209...
1203	fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;	1209	fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
1204	fehler += MesswertGier / 12;	1210	//fehler += MesswertGier / 12;
1205		1211
1206	if(!KompassSignalSchlecht && w < 25)	1212	if(!KompassSignalSchlecht && w < 25)
Line 1211...	Line 1217...
1211	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;	1217	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
1212	KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);	1218	KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
1213	NeueKompassRichtungMerken = 0;	1219	NeueKompassRichtungMerken = 0;
1214	}	1220	}
1215	}	1221	}
1216	DebugOut.Analog[16] = fehler;	1222	//DebugOut.Analog[16] = fehler;
1217	DebugOut.Analog[17] = korrektur;	1223	//DebugOut.Analog[17] = korrektur;
1218	ErsatzKompass += (fehler * 16) / korrektur;	1224	ErsatzKompass += (fehler * 16) / korrektur;
1219	w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren	1225	w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren
1220	w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln	1226	w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
1221	if(w >= 0)	1227	if(w >= 0)
1222	{	1228	{

Subversion Repositories FlightCtrl

(root)/branches/V0.80g_ACC-HH_MartinR_HR/fc.c @ 1993 - Rev 1662 → 1664