| 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]; |