Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 153 → Rev 155

/branches/salvo_gps/fc.c
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;
*/