Rev 893 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
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 |