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