96,7 → 96,7 |
uint8_t gps_cmd = GPS_CMD_STOP; |
static int ubat_cnt =0; |
static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung |
int w,v; |
int w,v,w1,v1; |
//Salvo End |
|
//Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation |
167,6 → 167,9 |
int MaxStickNick = 0,MaxStickRoll = 0; |
unsigned int modell_fliegt = 0; |
unsigned char MikroKopterFlags = 0; |
//Salvo 31.12.2008 |
long waagrecht; // Mass fuer Annaeherung an Waagrecht fuer Kompasskompensation |
// Salvo End |
|
//Salvo 2.1.2008 Allgemeine Debugvariablen |
int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7; |
1105,25 → 1108,31 |
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 |
w1 = abs((int)(IntegralNick / EE_Parameter.GyroAccFaktor)); //Abfrage auch gegen Gyro beiu Kurvenflug sonst evtl. vorgetäuschte waagrechte Lage |
v1 = abs((int)(IntegralRoll / EE_Parameter.GyroAccFaktor)); |
|
if ((w1<ACC_WAAGRECHT_LIMIT)&&(w<ACC_WAAGRECHT_LIMIT)&&(v<ACC_WAAGRECHT_LIMIT)&&(v1<ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok |
{ |
waagrecht = ((long)(ACC_WAAGRECHT_LIMIT-(long)w) * ((long)ACC_WAAGRECHT_LIMIT-(long)w)) *(((long)ACC_WAAGRECHT_LIMIT-(long)v)* ((long)ACC_WAAGRECHT_LIMIT-(long)v)); |
waagrecht = (waagrecht * 100) /((long)ACC_WAAGRECHT_LIMIT * (long)ACC_WAAGRECHT_LIMIT * (long)ACC_WAAGRECHT_LIMIT*(long)ACC_WAAGRECHT_LIMIT) ; //bei Waagrecht Wert = 100 |
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)GIER_GRAD_FAKTOR); |
v = KompassValue - gyrogier_kompass; //realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen |
w = (int) (GyroGier_Comp/((long)GIER_GRAD_FAKTOR)); |
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 = (v * (int) waagrecht)/100; //mit Neigung bewerten |
|
if (v > GIER_COMP_MAX) v= GIER_COMP_MAX; |
if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX; |
if (abs(w) > 1) |
if (abs(v) > 0) |
{ |
GyroGier_Comp = 0; |
gyrogier_kompass = KompassValue; // Kompasswert merken |
1139,27 → 1148,25 |
} |
|
magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet |
GyroKomp_Int = (GyroKomp_Int )/(long)GIER_GRAD_FAKTOR; |
// GyroKomp_Int = (GyroKomp_Int )/((long)GIER_GRAD_FAKTOR); |
|
w = KompassValue - GyroKomp_Int; |
if ((w > 0) && (w < 180)) |
w = KompassValue - (int)(GyroKomp_Int/(long)GIER_GRAD_FAKTOR); |
w = w%360; |
#define GYRO_KOMP_DELTA_MAX 10 |
if (w > GYRO_KOMP_DELTA_MAX) w = GYRO_KOMP_DELTA_MAX; //Begrenzung |
if (w <-GYRO_KOMP_DELTA_MAX) w =-GYRO_KOMP_DELTA_MAX; //Begrenzung |
|
if (((w > 0) && (w < 180)) || ((w < 0) && (w < -180))) |
{ |
++GyroKomp_Int; |
GyroKomp_Int = GyroKomp_Int + (((labs((long)w) * waagrecht)/100)*(long)GIER_GRAD_FAKTOR); // je waagrechter desto groesser die Korrektur |
} |
else if ((w > 0) && (w >= 180)) |
else if (((w > 0) && (w >= 180)) || ((w < 0) && (w >= -180))) |
{ |
--GyroKomp_Int; |
GyroKomp_Int = GyroKomp_Int - (((labs((long)w) * waagrecht)/100)*(long)GIER_GRAD_FAKTOR); |
} |
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)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern |
if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360L * (long)GIER_GRAD_FAKTOR; //Ueberlauf korrigieren |
if (GyroKomp_Int > 360L * (long)GIER_GRAD_FAKTOR) GyroKomp_Int = GyroKomp_Int - 360L * (long)GIER_GRAD_FAKTOR; |
// GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern |
} |
} |
else //Kompass wegen zu grosser Neigung ungueltig |
1298,8 → 1305,10 |
DebugOut.Analog[31] = GPS_Roll; |
// DebugOut.Analog[30] = KompassStartwert; |
// DebugOut.Analog[31] = KompassRichtung; |
// DebugOut.Analog[31] = (int) waagrecht; |
|
|
|
/* DebugOut.Analog[16] = motor_rx[0]; |
DebugOut.Analog[17] = motor_rx[1]; |
DebugOut.Analog[18] = motor_rx[2]; |