WebSVN - FlightCtrl - Diff - Rev 1702 and 1703



unsigned char h,m,s;
unsigned int BaroExpandActive = 0;
int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
int TrimNick, TrimRoll;
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
int Mittelwert_AccNick, Mittelwert_AccRoll;
unsigned int NeutralAccX=0, NeutralAccY=0;
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
int NeutralAccZ = 0;
unsigned char ControlHeading = 0;// in 2°
long IntegralNick = 0,IntegralNick2 = 0;

    DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
    DebugOut.Analog[2] = Mittelwert_AccNick / 4;
    DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
    DebugOut.Analog[4] = (signed int) AdNeutralGier - AdWertGier;
    DebugOut.Analog[5] = HoehenWert/5;
    DebugOut.Analog[6] = AdWertAccHoch;//(Mess_Integral_Hoch / 512);// Aktuell_az;
    DebugOut.Analog[8] = KompassValue;
    DebugOut.Analog[9] = UBat;
    DebugOut.Analog[10] = SenderOkay;
    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
    DebugOut.Analog[12] = Motor[0].SetPoint;

    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
        ANALOG_OFF;
        MesswertNick = AdWertNick;
        MesswertRoll = AdWertRoll;
        MesswertGier = AdWertGier;
        Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick;
        Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll;
 
   // ADC einschalten
    ANALOG_ON;
   for(i=0;i<8;i++)
    {
     int tmp;

 
    MesswertNick = 0;
    MesswertRoll = 0;
    MesswertGier = 0;
    Delay_ms_Mess(100);
    Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick;
    Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll;
    IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
    IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
    Mess_IntegralNick2 = IntegralNick;
    Mess_IntegralRoll2 = IntegralRoll;

    MesswertRoll = (signed int) AdWertRollFilter / 8;
    RohMesswertNick = MesswertNick;
    RohMesswertRoll = MesswertRoll;

 
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
        Mittelwert_AccNick = (Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * AdWertAccNick))) / 4L;
 
        Mittelwert_AccRoll = (Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * AdWertAccRoll))) / 4L;
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
    NaviAccNick    += AdWertAccNick;
    NaviAccRoll    += AdWertAccRoll;

    if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; }
    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; }
   }
    GierGyroFehler = 0;

 
 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL / 2)
#define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16
#define BEWEGUNGS_LIMIT 20000

Rev 1702	Rev 1703
Line 60...	Line 60...
60	unsigned char h,m,s;	60	unsigned char h,m,s;
61	unsigned int BaroExpandActive = 0;	61	unsigned int BaroExpandActive = 0;
62	int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;	62	int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
63	int TrimNick, TrimRoll;	63	int TrimNick, TrimRoll;
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;
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 ControlHeading = 0;// in 2°	69	unsigned char ControlHeading = 0;// in 2°
70	long IntegralNick = 0,IntegralNick2 = 0;	70	long IntegralNick = 0,IntegralNick2 = 0;
Line 173...	Line 173...
173	DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);	173	DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
174	DebugOut.Analog[2] = Mittelwert_AccNick / 4;	174	DebugOut.Analog[2] = Mittelwert_AccNick / 4;
175	DebugOut.Analog[3] = Mittelwert_AccRoll / 4;	175	DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
176	DebugOut.Analog[4] = (signed int) AdNeutralGier - AdWertGier;	176	DebugOut.Analog[4] = (signed int) AdNeutralGier - AdWertGier;
177	DebugOut.Analog[5] = HoehenWert/5;	177	DebugOut.Analog[5] = HoehenWert/5;
178	DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);//AdWertAccHoch;// Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;	178	DebugOut.Analog[6] = AdWertAccHoch;//(Mess_Integral_Hoch / 512);// Aktuell_az;
179	DebugOut.Analog[8] = KompassValue;	179	DebugOut.Analog[8] = KompassValue;
180	DebugOut.Analog[9] = UBat;	180	DebugOut.Analog[9] = UBat;
181	DebugOut.Analog[10] = SenderOkay;	181	DebugOut.Analog[10] = SenderOkay;
182	DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;	182	DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
183	DebugOut.Analog[12] = Motor[0].SetPoint;	183	DebugOut.Analog[12] = Motor[0].SetPoint;
Line 216...	Line 216...
216	// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern	216	// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
217	ANALOG_OFF;	217	ANALOG_OFF;
218	MesswertNick = AdWertNick;	218	MesswertNick = AdWertNick;
219	MesswertRoll = AdWertRoll;	219	MesswertRoll = AdWertRoll;
220	MesswertGier = AdWertGier;	220	MesswertGier = AdWertGier;
221	Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;	221	Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick;
222	Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;	222	Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll;
223	Mittelwert_AccHoch = (long)AdWertAccHoch;	-
224	// ADC einschalten	223	// ADC einschalten
225	ANALOG_ON;	224	ANALOG_ON;
226	for(i=0;i<8;i++)	225	for(i=0;i<8;i++)
227	{	226	{
228	int tmp;	227	int tmp;
Line 309...	Line 308...
309		308
310	MesswertNick = 0;	309	MesswertNick = 0;
311	MesswertRoll = 0;	310	MesswertRoll = 0;
312	MesswertGier = 0;	311	MesswertGier = 0;
313	Delay_ms_Mess(100);	312	Delay_ms_Mess(100);
314	Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;	313	Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick;
315	Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;	314	Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll;
316	IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;	315	IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
317	IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;	316	IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
318	Mess_IntegralNick2 = IntegralNick;	317	Mess_IntegralNick2 = IntegralNick;
319	Mess_IntegralRoll2 = IntegralRoll;	318	Mess_IntegralRoll2 = IntegralRoll;
Line 370...	Line 369...
370	MesswertRoll = (signed int) AdWertRollFilter / 8;	369	MesswertRoll = (signed int) AdWertRollFilter / 8;
371	RohMesswertNick = MesswertNick;	370	RohMesswertNick = MesswertNick;
372	RohMesswertRoll = MesswertRoll;	371	RohMesswertRoll = MesswertRoll;
Line 373...	Line 372...
373		372
374	// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++	373	// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++
375	Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L;	374	Mittelwert_AccNick = (Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * AdWertAccNick))) / 4L;
376	Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L;	-
377	Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 3 + ((long)AdWertAccHoch)) / 4L;	375	Mittelwert_AccRoll = (Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * AdWertAccRoll))) / 4L;
378	IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;	376	IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
379	IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;	377	IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
380	NaviAccNick += AdWertAccNick;	378	NaviAccNick += AdWertAccNick;
381	NaviAccRoll += AdWertAccRoll;	379	NaviAccRoll += AdWertAccRoll;
Line 1078...	Line 1076...
1078	if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; }	1076	if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; }
1079	if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; }	1077	if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; }
1080	}	1078	}
1081	GierGyroFehler = 0;	1079	GierGyroFehler = 0;
Line 1082...	Line -...
1082		-
1083		1080
1084	#define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2)	1081	#define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2)
1085	#define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4	1082	#define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4
1086	#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16	1083	#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16
1087	#define BEWEGUNGS_LIMIT 20000	1084	#define BEWEGUNGS_LIMIT 20000

Subversion Repositories FlightCtrl

(root)/tags/V0.88e/fc.c @ 2503 - Rev 1702 → 1703