89,6 → 89,7 |
char MotorenEin = 0; |
int HoehenWert = 0; |
int SollHoehe = 0; |
int w,v; |
|
float Kp = FAKTOR_P; |
float Ki = FAKTOR_I; |
601,12 → 602,23 |
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--; |
// Salvo 1.9.2007 ************************* |
// Abgleich Roll und Nick Gyro nur, wenn nahezu waagrechte Lage |
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
w = (abs(Mittelwert_AccNick)); |
v = (abs(Mittelwert_AccRoll)); |
ANALOG_ON; // ADC einschalten |
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) |
{ |
if(IntegralFehlerNick > 500/DRIFT_FAKTOR) AdNeutralNick++; |
if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
} |
// Salvo End |
|
// Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist *********************** |
// Ohne Kompass wird die Gyrodrift durch die Driftkompensation nur verschlimmert |
// Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert |
// Ohne Driftkompensation ist die Gierachse wesentlich stabiler |
if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht)) |
{ |
630,13 → 642,36 |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16; |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16; |
#define AUSGLEICH 500 |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
|
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
Mess_IntegralNick -= tmp_long; |
Mess_IntegralRoll -= tmp_long2; |
// Salvo 1.9.2007 Volle Korrektur nur wenn waagrechte Lage, sonst abgeschawecht ***************** |
w = (abs(Mittelwert_AccNick)); |
v = (abs(Mittelwert_AccRoll)); |
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) |
{ |
Mess_IntegralNick -= tmp_long; |
Mess_IntegralRoll -= tmp_long2; |
} |
else if ((w < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT)) |
{ |
Mess_IntegralNick -= tmp_long/8; |
Mess_IntegralRoll -= tmp_long/8; |
} |
else if ((w < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT)) |
{ |
Mess_IntegralNick -= tmp_long/16; |
Mess_IntegralRoll -= tmp_long/16; |
} |
else |
{ |
Mess_IntegralNick -= tmp_long/32; |
Mess_IntegralRoll -= tmp_long2/32; |
} |
// Salvo End *********************** |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
660,7 → 695,6 |
//KompassValue = 12; |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
{ |
int w,v; |
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /512); |
if(v > w) w = v; // grösste Neigung ermitteln |
712,7 → 746,8 |
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
DebugOut.Analog[7] = GasMischanteil; |
DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[9] = Mess_Integral_Gier2; |
DebugOut.Analog[9] = tmp_long; |
DebugOut.Analog[10] = tmp_long2; |
// DebugOut.Analog[9] = SollHoehe; |
// DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
// DebugOut.Analog[11] = KompassStartwert; |