Rev 1101 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1101 | Rev 1102 | ||
---|---|---|---|
Line 100... | Line 100... | ||
100 | //Salvo End |
100 | //Salvo End |
Line 101... | Line 101... | ||
101 | 101 | ||
102 | //Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation |
102 | //Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation |
103 | long GyroKomp_Int; |
103 | long GyroKomp_Int; |
104 | long int GyroGier_Comp; |
104 | long int GyroGier_Comp; |
105 | int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass |
105 | //int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass |
106 | short int cnt_stickgier_zero =0; |
106 | short int cnt_stickgier_zero =0; |
107 | int gyrogier_kompass; |
107 | int gyrogier_kompass; |
Line 108... | Line 108... | ||
108 | //Salvo End |
108 | //Salvo End |
Line 171... | Line 171... | ||
171 | //Salvo 2.1.2008 Allgemeine Debugvariablen |
171 | //Salvo 2.1.2008 Allgemeine Debugvariablen |
172 | 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; |
172 | 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; |
173 | //Salvo End |
173 | //Salvo End |
Line 174... | Line -... | ||
174 | - | ||
175 | - | ||
176 | 174 | ||
177 | 175 | ||
178 | void Piep(unsigned char Anzahl) |
176 | void Piep(unsigned char Anzahl) |
179 | { |
177 | { |
180 | while(Anzahl--) |
178 | while(Anzahl--) |
Line 486... | Line 484... | ||
486 | CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); |
484 | CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); |
487 | CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
485 | CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
488 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); |
486 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); |
489 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
487 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
490 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
488 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
491 | - | ||
492 | CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255); |
489 | CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255); |
493 | CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255); |
490 | CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255); |
494 | CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255); |
491 | CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255); |
495 | CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255); |
492 | CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255); |
496 | CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255); |
493 | CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255); |
Line 1100... | Line 1097... | ||
1100 | MittelIntegralRoll2 = 0; |
1097 | MittelIntegralRoll2 = 0; |
1101 | ZaehlMessungen = 0; |
1098 | ZaehlMessungen = 0; |
1102 | } |
1099 | } |
1103 | //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
1100 | //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
Line 1104... | Line 1101... | ||
1104 | 1101 | ||
1105 | // Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 ********************** |
1102 | // Salvo Ersatzkompass und Giergyrokompensation 30.12.2007 ********************** |
1106 | if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat |
1103 | if ((Kompass_Neuer_Wert > 0) && (Kompass_present>0)) //nur wenn Kompass einen neuen Wert geliefert hat |
1107 | { |
1104 | { |
1108 | Kompass_Neuer_Wert = 0; |
1105 | Kompass_Neuer_Wert = 0; |
1109 | w = (abs(Mittelwert_AccNick)); |
1106 | w = (abs(Mittelwert_AccNick)); |
1110 | v = (abs(Mittelwert_AccRoll)); |
1107 | v = (abs(Mittelwert_AccRoll)); |
1111 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT) && (Kompass_present>0)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok |
1108 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok |
1112 | { |
1109 | { |
1113 | if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
1110 | if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
1114 | { |
1111 | { |
1115 | if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass |
1112 | if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass |
1116 | { |
1113 | { |
1117 | if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1; |
1114 | if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1; |
1118 | if (cnt_stickgier_zero > 10) // nur Abgleichen wenn keine Stickbewegung da |
1115 | if (cnt_stickgier_zero > 1) //nur Abgleichen wenn keine Stickbewegung da |
1119 | { |
1116 | { |
1120 | w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR); |
1117 | w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR); |
1121 | v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen |
1118 | v = KompassValue - gyrogier_kompass; //realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen |
1122 | if (v <-180) v +=360; // Uberlaufkorrektur |
1119 | if (v <-180) v +=360; // Uberlaufkorrektur |
Line 1123... | Line 1120... | ||
1123 | if (v > 180) v -=360; // Uberlaufkorrektur |
1120 | if (v > 180) v -=360; // Uberlaufkorrektur |
Line 1124... | Line -... | ||
1124 | - | ||
1125 | v = w-v; //Differenz Gyro zu Kompass ist der Driftfehler |
1121 | |
1126 | 1122 | v = w-v; //Differenz Gyro zu Kompass ist der Driftfehler |
|
1127 | #define GIER_COMP_MAX 4 |
1123 | |
1128 | if (v > GIER_COMP_MAX) v= GIER_COMP_MAX; |
1124 | if (v > GIER_COMP_MAX) v= GIER_COMP_MAX; |
1129 | if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX; |
1125 | if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX; |
Line 1164... | Line 1160... | ||
1164 | } |
1160 | } |
1165 | if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360L; |
1161 | if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360L; |
1166 | GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern |
1162 | GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern |
1167 | } |
1163 | } |
1168 | } |
1164 | } |
1169 | else //Kompass wegen grosser Neigung ungueltig |
1165 | else //Kompass wegen zu grosser Neigung ungueltig |
1170 | { |
1166 | { |
1171 | magkompass_ok = 0; |
1167 | magkompass_ok = 0; |
1172 | GyroGier_Comp = 0; |
1168 | GyroGier_Comp = 0; |
1173 | } |
1169 | } |
1174 | Kompass_Value_Old =KompassValue; |
1170 | Kompass_Value_Old =KompassValue; |