| 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; |