| 448,7 → 448,6 |
|---|
| { |
| ROT_ON; //led blitzen lassen |
| } |
| |
| //******PROVISORISCH*************** |
| GRN_ON; |
| |
| 641,27 → 640,31 |
|---|
| 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--; |
| } |
| else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht |
| // Abgleich Roll und Nick Gyro nur wenn auch keine wilden Manoever stattfinden 26.9.2007 |
| if ((abs(StickNick - GPS_Nick) < 30) && (abs(StickRoll - GPS_Roll) < 30)) |
| { |
| if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR) AdNeutralNick++; |
| if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR) AdNeutralNick--; |
| if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR) AdNeutralRoll++; |
| if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR) AdNeutralRoll--; |
| 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--; |
| } |
| else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht |
| { |
| if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR) AdNeutralNick++; |
| if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR) AdNeutralNick--; |
| if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR) AdNeutralRoll++; |
| if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR) AdNeutralRoll--; |
| } |
| else // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht |
| { |
| if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR) AdNeutralNick++; |
| if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR) AdNeutralNick--; |
| if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR) AdNeutralRoll++; |
| if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR) AdNeutralRoll--; |
| } |
| } |
| else // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht |
| { |
| if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR) AdNeutralNick++; |
| if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR) AdNeutralNick--; |
| if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR) AdNeutralRoll++; |
| if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR) AdNeutralRoll--; |
| } |
| // Salvo End |
| |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| 683,6 → 686,7 |
|---|
| |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| // 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)) |
| 723,7 → 727,7 |
|---|
| // Salvo End ************************* |
| ANALOG_ON; // ADC einschalten |
| |
| // Salvo Ersatzkompass 8.9.2007 ********************** |
| // Salvo Ersatzkompass 26.9.2007 ********************** |
| if ((Kompass_Neuer_Wert > 0)) |
| { |
| Kompass_Neuer_Wert = 0; |
| 731,7 → 735,7 |
|---|
| v = (abs(Mittelwert_AccRoll)); |
| if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alle sok |
| { |
| if ((abs(KompassValue - Kompass_Value_Old)) < 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
| if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
| { |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet |
| 754,8 → 758,7 |
|---|
| ++GyroKomp_Int; |
| } |
| if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360; |
| |
| GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern |
| GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern |
| ANALOG_ON; // ADC einschalten |
| } |
| } |