76,9 → 76,10 |
unsigned char Notlandung = 0; |
unsigned char HoehenReglerAktiv = 0; |
static int SignalSchlecht = 0; |
uint8_t magkompass_ok=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; |
177,7 → 178,6 |
//Salvo 2.9.2007 Ersatzkompass |
GyroKomp_Int = 0; |
GyroKomp_Int2 = 0; |
GyroKomp_Inc_Grad = GYROKOMP_INC_GRAD_DEFAULT; |
// Salvo End |
} |
|
321,8 → 321,8 |
EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250 |
EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
EE_Parameter.NotGas = 90; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 50; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
EE_Parameter.NotGas = 100; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
EE_Parameter.I_Faktor = 5; |
EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
364,8 → 364,8 |
EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250 |
EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
EE_Parameter.NotGas = 90; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 50; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
EE_Parameter.NotGas = 100; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
EE_Parameter.I_Faktor = 5; |
EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
616,13 → 616,9 |
#define DRIFT_FAKTOR 3 |
if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
{ |
// Salvo 8.9.2007 Ersatzkompass ******* |
// Salvo 12.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; |
} |
if (GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT >=360 ) GyroKomp_Int = 0; |
ANALOG_ON; // ADC einschalten |
ROT_OFF; |
// Salvo End |
662,15 → 658,15 |
// Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist *********************** |
// 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)) |
{ |
if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; |
if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; |
} |
else |
{ |
Mess_Integral_Gier2 = 0; |
} |
// if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht)) |
// Salvo 13.9.2007 |
// if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (magkompass_ok >0)) |
// { |
// if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; |
// if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; |
// } |
// else Mess_Integral_Gier2 = 0; |
|
// Salvo End *********************** |
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
Mess_IntegralNick2 = IntegralNick; |
718,7 → 714,7 |
// Gieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
sollGier = StickGier; |
if(abs(StickGier) > 35) |
if(abs(StickGier) > 30) |
{ |
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
} |
734,13 → 730,15 |
// Salvo Ersatzkompass 8.9.2007 ********************** |
if (Kompass_Neuer_Wert > 0) |
{ |
w = (abs(Mittelwert_AccNick)); |
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 |
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet |
GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT; |
w = KompassValue - GyroKomp_Int; |
if ((w > 0) && (w < 180)) |
765,7 → 763,7 |
ANALOG_ON; // ADC einschalten |
} |
} |
Kompass_Neuer_Wert = 0; |
else magkompass_ok = 0; |
} |
|
// Salvo End ************************* |
774,17 → 772,29 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//KompassValue = 12; |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
{ |
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /512); |
if(v > w) w = v; // grösste Neigung ermitteln |
if(w < 20 && NeueKompassRichtungMerken && !SignalSchlecht) |
if(v > w) w = v; // grösste Neigung ermitteln |
// Salvo 12.9.2007 Kompassdaempfung abschalten weil Ersatzkompass verwendet wird |
// w=0, v=0; |
//Salvo End |
// if(w < 20 && NeueKompassRichtungMerken && !SignalSchlecht) |
if ((magkompass_ok > 0) && NeueKompassRichtungMerken) |
{ |
KompassStartwert = KompassValue; |
// Salvo 12.9.2007 Ersatzkompass nehmen statt Magnetkompass |
// KompassStartwert = KompassValue; |
// KompassStartwert = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
// Salvo End ************************* |
// Salvo 13.9.2007 |
KompassStartwert = KompassValue; |
// Salvo End |
NeueKompassRichtungMerken = 0; |
} |
// Salvo 13.9.2007 |
w=0; |
// Salvo End |
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if(w > 0) |
791,7 → 801,11 |
{ |
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
|
// Salvo 30.8.2007 Winkelbegrenzung ********************** |
// Salvo Kompasssteuerung ********************** |
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
// Salvo End |
// Salvo 30.8.2007 Winkelbegrenzung ********************** |
/* |
if ((!SignalSchlecht) ) |
{ |
if (abs(KompassRichtung) < 135 ) |
799,6 → 813,7 |
Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
} |
} |
*/ |
// Salvo End ************************* |
|
ANALOG_ON; // ADC einschalten |
829,16 → 844,18 |
DebugOut.Analog[7] = GasMischanteil; |
DebugOut.Analog[8] = KompassValue; |
// ******provisorisch |
DebugOut.Analog[9] = cnt1; |
// DebugOut.Analog[9] = cnt1; |
// DebugOut.Analog[10] = cnt1; |
// DebugOut.Analog[11] = cnt2; |
DebugOut.Analog[10] = (gps_act_position.utm_east/10) % 10000; |
DebugOut.Analog[11] = (gps_act_position.utm_north/10) % 10000; |
// DebugOut.Analog[10] = (gps_act_position.utm_east/10) % 10000; |
// DebugOut.Analog[11] = (gps_act_position.utm_north/10) % 10000; |
// ******provisorisch |
|
/* |
|
DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
DebugOut.Analog[10] = GyroKomp_Int2/GYROKOMP_INC_GRAD_DEFAULT; |
DebugOut.Analog[10] = magkompass_ok; |
DebugOut.Analog[11] = Mess_Integral_Gier2; |
/* |
DebugOut.Analog[11] = GyroKomp_Inc_Grad; |
DebugOut.Analog[12] = GyroKomp_Value; |
*/ |