Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 578 → Rev 579

/branches/salvo_gps/Basis_v0067g/trunk/fc.c
471,7 → 471,7
if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
//Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen
// und dieser dann langsam zwangsweise reduziert
if (UBat <= EE_Parameter.UnterspannungsWarnung - 2) //Unterhalb der Piepser Schwelle aktivieren
if (UBat <= EE_Parameter.UnterspannungsWarnung - 4) //Unterhalb der Piepser Schwelle aktivieren
{
if (ubat_cnt > 1000)
{
940,10 → 940,10
else last_n_n = 1;
} else last_n_n = 0;
} else cnt = 0;
//Salvo 11.12.2007
//Salvo 26.12.2007
w = (abs(Mittelwert_AccNick));
v = (abs(Mittelwert_AccRoll));
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in wwagrechter Lage nachtrimmen
if ((w < ACC_WAAGRECHT_LIMIT*2) && (v < ACC_WAAGRECHT_LIMIT*2)) // Gyro nur in annaehend waagrechter Lage nachtrimmen
{
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
982,15 → 982,13
{
cnt = 0;
}
//Salvo 11.12.2007
w = (abs(Mittelwert_AccNick));
v = (abs(Mittelwert_AccRoll));
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in wwagrechter Lage nachtrimmen
//Salvo 26.12.2007
if ((w < ACC_WAAGRECHT_LIMIT*2) && (v < ACC_WAAGRECHT_LIMIT*2)) // Gyro nur inannaehernd waagrechter Lage nachtrimmen
{
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
}
}
//Salvo End
//DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
//DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
1005,90 → 1003,88
MittelIntegralNick_Alt = MittelIntegralNick;
MittelIntegralRoll_Alt = MittelIntegralRoll;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
IntegralAccNick = 0;
IntegralAccRoll = 0;
IntegralAccZ = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
ZaehlMessungen = 0;
 
 
IntegralAccNick = 0;
IntegralAccRoll = 0;
IntegralAccZ = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
ZaehlMessungen = 0;
} // Ende Abgleich
 
// Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 **********************
if ((Kompass_Neuer_Wert > 0))
// Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 **********************
if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat
{
Kompass_Neuer_Wert = 0;
w = (abs(Mittelwert_AccNick));
v = (abs(Mittelwert_AccRoll));
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok
{
if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
{
Kompass_Neuer_Wert = 0;
w = (abs(Mittelwert_AccNick));
v = (abs(Mittelwert_AccRoll));
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok
{
if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
{
 
if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass
if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass
{
if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;
if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da
{
if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;
if (cnt_stickgier_zero > 1) // nur Abgleichen wenn keine Stickbewegung da
{
w = (int) (GyroGier_Comp/(long)GYROKOMP_INC_GRAD_DEFAULT);
v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
if (v <-180) v +=360; // Uberlaufkorrektur
if (v > 180) v -=360; // Uberlaufkorrektur
w = (int) (GyroGier_Comp/(long)GYROKOMP_INC_GRAD_DEFAULT);
v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
if (v <-180) v +=360; // Uberlaufkorrektur
if (v > 180) v -=360; // Uberlaufkorrektur
 
v = w -v; //Differenz Gyro zu Kompass ist der Driftfehler
v = w-v; //Differenz Gyro zu Kompass ist der Driftfehler
 
#define GIER_COMP_MAX 4
if (v > GIER_COMP_MAX) v= GIER_COMP_MAX;
if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
if (abs(w) > 1)
{
GyroGier_Comp = 0;
gyrogier_kompass = KompassValue; // Kompasswert merken
AdNeutralGier -= v;
}
#define GIER_COMP_MAX 8
if (v > GIER_COMP_MAX) v= GIER_COMP_MAX;
if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
if (abs(w) > 1)
{
GyroGier_Comp = 0;
gyrogier_kompass = KompassValue; // Kompasswert merken
AdNeutralGier -= v;
}
}
else
{
gyrogier_kompass = KompassValue; // Kompasswert merken
cnt_stickgier_zero = 0;
GyroGier_Comp = 0;
}
}
else
{
gyrogier_kompass = KompassValue; // Kompasswert merken
cnt_stickgier_zero = 0;
GyroGier_Comp = 0;
}
 
magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
GyroKomp_Int = (GyroKomp_Int )/(long)GYROKOMP_INC_GRAD_DEFAULT;
magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
GyroKomp_Int = (GyroKomp_Int )/(long)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 + 360L;
GyroKomp_Int = (GyroKomp_Int%360L) * (long)GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
}
}
else
{
magkompass_ok = 0;
GyroGier_Comp = 0;
}
Kompass_Value_Old = KompassValue;
}
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 + 360L;
GyroKomp_Int = (GyroKomp_Int%360L) * (long)GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
}
}
else //Kompassfehler
{
magkompass_ok = 0;
GyroGier_Comp = 0;
}
Kompass_Value_Old = KompassValue;
}
// Salvo End *************************
 
// Salvo 6.10.2007
1207,8 → 1203,10
DebugOut.Analog[27] = gps_rel_act_position.utm_north;
DebugOut.Analog[28] = gps_rel_act_position.utm_alt;
DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd);
DebugOut.Analog[30] = GPS_dist_2trgt;
DebugOut.Analog[31] = (int) GyroGier_Comp;
DebugOut.Analog[30] = dist_flown;
DebugOut.Analog[31] = (int) dist_2home;
// DebugOut.Analog[31] = (int) GPS_hdng_abs_2trgt;
// DebugOut.Analog[31] = (int) GyroGier_Comp;
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];