WebSVN - FlightCtrl - Diff - Rev 893 and 902 - /branches/MicroMag3



volatile unsigned int I2CTimeout = 100;
volatile int MesswertNick,MesswertRoll,MesswertGier;
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;
int NaviAccNick, NaviAccRoll,NaviCntAcc = 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(AdWertNick > 2020) MesswertNick = +1000;
              if(AdWertNick > 2034) MesswertNick = +2000;
                         }
//++++++++++++++++++++++++++++++++++++++++++++++++
// Neue ADC-Runde starten
    ADC_START; 
//++++++++++++++++++++++++++++++++++++++++++++++++

 
    Integral_Gier  = Mess_Integral_Gier;
    IntegralNick = Mess_IntegralNick;

//############################################################################
// Messwerte beim Ermitteln der Nullage
void CalibrierMittelwert(void)
//############################################################################
{                
    // IRQ auschalten, damit die Werte sich nicht während der Berechnung ändern
        uint8_t tmp_sreg = SREG;
    cli();
         MesswertNick = AdWertNick;
         MesswertRoll = AdWertRoll;
         MesswertGier = AdWertGier;
         Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
         Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
         Mittelwert_AccHoch = (long)AdWertAccHoch;
    // IRQ einschalten
    SREG = tmp_sreg;

       
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--;
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--;
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--;

                           WinkelOut.CalcState = 1;
                           beeptime = 1000;
                          }
                          else
                          {
                               ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], sizeof(struct mk_param_struct));
                           if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
                            {
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
                            }  
                           SetNeutral();

// Roll-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L);
    if(SummeRoll < -(STICK_GAIN * 16000L)) SummeRoll = -(STICK_GAIN * 16000L);
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
    // Motor Links

Rev 893	Rev 902
Line 59...	Line 59...
59	volatile unsigned int I2CTimeout = 100;	59	volatile unsigned int I2CTimeout = 100;
60	volatile int MesswertNick,MesswertRoll,MesswertGier;	60	volatile int MesswertNick,MesswertRoll,MesswertGier;
61	volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;	61	volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
62	volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;	62	volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;
63	int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;	63	int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
64	unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;	-
65	long IntegralNick = 0,IntegralNick2 = 0;	64	long IntegralNick = 0,IntegralNick2 = 0;
66	long IntegralRoll = 0,IntegralRoll2 = 0;	65	long IntegralRoll = 0,IntegralRoll2 = 0;
67	long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;	66	long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
68	long Integral_Gier = 0;	67	long Integral_Gier = 0;
69	long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;	68	long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
Line 318...	Line 317...
318	if(AdWertNick > 2020) MesswertNick = +1000;	317	if(AdWertNick > 2020) MesswertNick = +1000;
319	if(AdWertNick > 2034) MesswertNick = +2000;	318	if(AdWertNick > 2034) MesswertNick = +2000;
320	}	319	}
321	//++++++++++++++++++++++++++++++++++++++++++++++++	320	//++++++++++++++++++++++++++++++++++++++++++++++++
322	// Neue ADC-Runde starten	321	// Neue ADC-Runde starten
323	ANALOG_ON;	322	ADC_START;
324	//++++++++++++++++++++++++++++++++++++++++++++++++	323	//++++++++++++++++++++++++++++++++++++++++++++++++
Line 325...	Line 324...
325		324
326	Integral_Gier = Mess_Integral_Gier;	325	Integral_Gier = Mess_Integral_Gier;
327	IntegralNick = Mess_IntegralNick;	326	IntegralNick = Mess_IntegralNick;
Line 346...	Line 345...
346	//############################################################################	345	//############################################################################
347	// Messwerte beim Ermitteln der Nullage	346	// Messwerte beim Ermitteln der Nullage
348	void CalibrierMittelwert(void)	347	void CalibrierMittelwert(void)
349	//############################################################################	348	//############################################################################
350	{	349	{
351	// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern	350	// IRQ auschalten, damit die Werte sich nicht während der Berechnung ändern
-		351	uint8_t tmp_sreg = SREG;
352	ANALOG_OFF;	352	cli();
353	MesswertNick = AdWertNick;	353	MesswertNick = AdWertNick;
354	MesswertRoll = AdWertRoll;	354	MesswertRoll = AdWertRoll;
355	MesswertGier = AdWertGier;	355	MesswertGier = AdWertGier;
356	Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;	356	Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
357	Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;	357	Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
358	Mittelwert_AccHoch = (long)AdWertAccHoch;	358	Mittelwert_AccHoch = (long)AdWertAccHoch;
359	// ADC einschalten	359	// IRQ einschalten
360	ANALOG_ON;	360	SREG = tmp_sreg;
Line 361...	Line 361...
361		361
362	if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--;	362	if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--;
363	if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--;	363	if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--;
364	if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--;	364	if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--;
Line 553...	Line 553...
553	WinkelOut.CalcState = 1;	553	WinkelOut.CalcState = 1;
554	beeptime = 1000;	554	beeptime = 1000;
555	}	555	}
556	else	556	else
557	{	557	{
558	ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);	558	ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], sizeof(struct mk_param_struct));
559	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?	559	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
560	{	560	{
561	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();	561	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();
562	}	562	}
563	SetNeutral();	563	SetNeutral();
Line 1225...	Line 1225...
1225	// Roll-Achse	1225	// Roll-Achse
1226	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1226	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1227	DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen	1227	DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
1228	if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung	1228	if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung
1229	else SummeRoll += DiffRoll; // I-Anteil bei HH	1229	else SummeRoll += DiffRoll; // I-Anteil bei HH
1230	if(SummeRoll > 16000) SummeRoll = 16000;	1230	if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L);
1231	if(SummeRoll < -16000) SummeRoll = -16000;	1231	if(SummeRoll < -(STICK_GAIN * 16000L)) SummeRoll = -(STICK_GAIN * 16000L);
1232	pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll	1232	pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll
1233	tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;	1233	tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1234	if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;	1234	if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
1235	if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;	1235	if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
1236	// Motor Links	1236	// Motor Links

Subversion Repositories FlightCtrl

(root)/branches/MicroMag3_Nick666/V0.69k/fc.c - Rev 893 → 902