656,10 → 656,10 |
IntegralFehlerNick = IntegralNick2 - IntegralNick; |
IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
ZaehlMessungen = 0; |
if(IntegralFehlerNick > 500/DRIFT_FAKTOR) AdNeutralNick++; |
if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
if(IntegralFehlerNick > 800/DRIFT_FAKTOR) AdNeutralNick++; |
if(IntegralFehlerNick < -800/DRIFT_FAKTOR) AdNeutralNick--; |
if(IntegralFehlerRoll > 800/DRIFT_FAKTOR) AdNeutralRoll++; |
if(IntegralFehlerRoll < -800/DRIFT_FAKTOR) AdNeutralRoll--; |
// if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; // macht nur mit Referenz (Kompass Sinn) |
// if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; // macht nur mit Referenz (Kompass Sinn) |
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
671,16 → 671,14 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Integrale auf ACC-Signal abgleichen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(IntegralFaktor && !Looping_Nick && !Looping_Roll) |
if(!Looping_Nick && !Looping_Roll) |
{ |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
if(labs(Mittelwert_AccNick) < 200) tmp_long /= 8; |
else tmp_long /= 16; |
tmp_long /= 32; |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
if(labs(Mittelwert_AccRoll) < 200) tmp_long2 /= 8; |
else tmp_long2 /= 16; |
tmp_long2 /= 32; |
|
#define AUSGLEICH 500 |
#define AUSGLEICH 100 |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
752,6 → 750,8 |
DebugOut.Analog[5] = HoehenWert; |
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = SenderOkay; |
|
/* DebugOut.Analog[16] = motor_rx[0]; |
DebugOut.Analog[17] = motor_rx[1]; |
867,7 → 867,7 |
if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1; |
if(SummeNick > 16000) SummeNick = 16000; |
if(SummeNick < -16000) SummeNick = -16000; |
pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick |
pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
// Motor Vorn |
#define MUL 2 |
if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
892,7 → 892,7 |
if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1; |
if(SummeRoll > 16000) SummeRoll = 16000; |
if(SummeRoll < -16000) SummeRoll = -16000; |
pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll |
pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil)); |
// Motor Links |