| 78,6 → 78,8 |
|---|
| static int SignalSchlecht = 0; |
| //Salvo 2.9.2007 Ersatzkompass |
| volatile long GyroKomp_Int,GyroKomp_Int2; |
| volatile int GyroKomp_Inc_Grad; // Gyroincrements / Grad |
| volatile int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass |
| // Salvo End |
| float GyroFaktor; |
| float IntegralFaktor; |
| 175,6 → 177,7 |
|---|
| //Salvo 2.9.2007 Ersatzkompass |
| GyroKomp_Int = 0; |
| GyroKomp_Int2 = 0; |
| GyroKomp_Inc_Grad = GYROKOMP_INC_GRAD_DEFAULT; |
| // Salvo End |
| } |
| |
| 605,6 → 608,16 |
|---|
| #define DRIFT_FAKTOR 3 |
| if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
| { |
| // Salvo 8.9.2007 Ersatzkompass ******* |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| if (GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT >=360 ) |
| { |
| // GyroKomp_Int = GyroKomp_Int- (long)(GYROKOMP_INC_GRAD_DEFAULT *360); |
| GyroKomp_Int = 0; |
| } |
| ANALOG_ON; // ADC einschalten |
| // Salvo End |
| |
| IntegralFehlerNick = IntegralNick2 - IntegralNick; |
| IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
| ZaehlMessungen = 0; |
| 709,6 → 722,46 |
|---|
| // Salvo End ************************* |
| ANALOG_ON; // ADC einschalten |
| |
| // Salvo Ersatzkompass 8.9.2007 ********************** |
| if (Kompass_Neuer_Wert > 0) |
| { |
| w = (abs(Mittelwert_AccNick)); |
| 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 |
| { |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT; |
| w = KompassValue - GyroKomp_Int; |
| if ((w > 0) && (w < 180)) |
| { |
| ++GyroKomp_Int; |
| } |
| else if ((w > 0) && (w >= 180)) |
| { |
| --GyroKomp_Int; |
| } |
| else if ((w < 0) && (w >= -180)) |
| { |
| --GyroKomp_Int; |
| } |
| else if ((w < 0) && (w < -180)) |
| { |
| ++GyroKomp_Int; |
| } |
| if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360; |
| |
| GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern |
| ANALOG_ON; // ADC einschalten |
| } |
| } |
| Kompass_Neuer_Wert = 0; |
| } |
| |
| // Salvo End ************************* |
| |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Kompass |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 766,8 → 819,10 |
|---|
| DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
| DebugOut.Analog[7] = GasMischanteil; |
| DebugOut.Analog[8] = KompassValue; |
| DebugOut.Analog[9] = GyroKomp_Int; |
| DebugOut.Analog[10] = GyroKomp_Int2; |
| DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
| DebugOut.Analog[10] = GyroKomp_Int2/GYROKOMP_INC_GRAD_DEFAULT; |
| DebugOut.Analog[11] = GyroKomp_Inc_Grad; |
| DebugOut.Analog[12] = GyroKomp_Value; |
| |
| // DebugOut.Analog[9] = SollHoehe; |
| // DebugOut.Analog[10] = Mess_Integral_Gier / 128; |