Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 552 → Rev 553

/branches/salvo_gps/Basis_v0067g/trunk/fc.c
94,9 → 94,12
int w,v;
//Salvo End
 
//Salvo 2.9.2007 Ersatzkompass
volatile long GyroKomp_Int;
volatile int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass
//Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation
long GyroKomp_Int;
long int GyroGier_Comp;
int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass
short int cnt_stickgier_zero =0;
int gyrogier_kompass;
// Salvo End
 
float GyroFaktor;
200,6 → 203,7
MesswertNick = 0;
MesswertRoll = 0;
MesswertGier = 0;
GyroGier_Comp =0;
StartLuftdruck = Luftdruck;
HoeheD = 0;
Mess_Integral_Hoch = 0;
237,7 → 241,8
IntegralAccZ += Aktuell_az - NeutralAccZ;
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
//Salvo 12.11.2007
GyroKomp_Int += MesswertGier;
GyroKomp_Int += (long)MesswertGier;
GyroGier_Comp += (long)MesswertGier;
//Salvo End
Mess_Integral_Gier += MesswertGier;
Mess_Integral_Gier2 += MesswertGier;
997,35 → 1002,82
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
ZaehlMessungen = 0;
}
ZaehlMessungen = 0;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(abs(StickGier) > 20) // war 35
{
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
}
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
sollGier = tmp_int;
Mess_Integral_Gier -= tmp_int;
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
// Salvo Ersatzkompass 26.9.2007 **********************
 
// Salvo 6.10.2007
// GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
//GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < GPS_STICK_HOLDOFF)
&& (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
{
if (Parameter_MaxHoehe > 200)
{
if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd = GPS_CMD_REQ_HOME;
n = GPS_CRTL(gps_cmd);
}
else
{
if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd = GPS_CMD_REQ_HOLD;
n= GPS_CRTL(gps_cmd);
}
}
else
{
if (gps_cmd != GPS_CMD_STOP)
{
gps_cmd = GPS_CMD_STOP;
n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden
}
}
} // Ende Abgleich
 
// Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 **********************
if ((Kompass_Neuer_Wert > 0))
{
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 alles ok
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 (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 = w -v; //Differenz Gyro zu Kompass ist der Driftfehler
 
#define GIER_COMP_MAX 4
if (w > GIER_COMP_MAX) w= GIER_COMP_MAX;
if (w < -GIER_COMP_MAX) w= - GIER_COMP_MAX;
if (!(w == 0))
{
GyroGier_Comp = 0;
gyrogier_kompass = KompassValue; // Kompasswert merken
AdNeutralGier -= w;
}
}
}
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 )/GYROKOMP_INC_GRAD_DEFAULT;
GyroKomp_Int = (GyroKomp_Int )/(long)GYROKOMP_INC_GRAD_DEFAULT;
 
w = KompassValue - GyroKomp_Int;
if ((w > 0) && (w < 180))
{
1043,43 → 1095,36
{
++GyroKomp_Int;
}
if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360;
GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
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;
}
else
{
magkompass_ok = 0;
GyroGier_Comp = 0;
}
Kompass_Value_Old = KompassValue;
}
// Salvo End *************************
 
// Salvo 6.10.2007
// GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
//GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < GPS_STICK_HOLDOFF)
&& (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
{
if (Parameter_MaxHoehe > 200)
{
if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd = GPS_CMD_REQ_HOME;
n = GPS_CRTL(gps_cmd);
}
else
{
if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd = GPS_CMD_REQ_HOLD;
n= GPS_CRTL(gps_cmd);
}
}
else
{
if (gps_cmd != GPS_CMD_STOP)
{
gps_cmd = GPS_CMD_STOP;
n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden
}
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(abs(StickGier) > 20) // war 35
{
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
}
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
sollGier = tmp_int;
Mess_Integral_Gier -= tmp_int;
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
 
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0))
1105,6 → 1150,7
}
 
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1112,6 → 1158,13
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!TimerWerteausgabe--)
{
 
// Salvo 13.12.2007 Beleuchtung steuern
if (!(beeptime & BeepMuster)) LED_J16_FLASH;
else if (MotorenEin) LED_J16_ON;
else LED_J16_OFF;
// Salvo End
TimerWerteausgabe = 24;
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
1143,6 → 1196,7
DebugOut.Analog[27] = gps_rel_act_position.utm_north/10;
DebugOut.Analog[28] = gps_rel_act_position.utm_alt/10;
DebugOut.Analog[29] = gps_sub_state+(20*gps_cmd);
DebugOut.Analog[31] = (int) GyroGier_Comp;
 
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];