Rev 1702 | Rev 1705 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1702 | Rev 1703 | ||
---|---|---|---|
Line 60... | Line 60... | ||
60 | unsigned char h,m,s; |
60 | unsigned char h,m,s; |
61 | unsigned int BaroExpandActive = 0; |
61 | unsigned int BaroExpandActive = 0; |
62 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
62 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
63 | int TrimNick, TrimRoll; |
63 | int TrimNick, TrimRoll; |
64 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
64 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
65 | int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch; |
65 | int Mittelwert_AccNick, Mittelwert_AccRoll; |
66 | unsigned int NeutralAccX=0, NeutralAccY=0; |
66 | unsigned int NeutralAccX=0, NeutralAccY=0; |
67 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
67 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
68 | int NeutralAccZ = 0; |
68 | int NeutralAccZ = 0; |
69 | unsigned char ControlHeading = 0;// in 2° |
69 | unsigned char ControlHeading = 0;// in 2° |
70 | long IntegralNick = 0,IntegralNick2 = 0; |
70 | long IntegralNick = 0,IntegralNick2 = 0; |
Line 173... | Line 173... | ||
173 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
173 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
174 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
174 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
175 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
175 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
176 | DebugOut.Analog[4] = (signed int) AdNeutralGier - AdWertGier; |
176 | DebugOut.Analog[4] = (signed int) AdNeutralGier - AdWertGier; |
177 | DebugOut.Analog[5] = HoehenWert/5; |
177 | DebugOut.Analog[5] = HoehenWert/5; |
178 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);//AdWertAccHoch;// Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
178 | DebugOut.Analog[6] = AdWertAccHoch;//(Mess_Integral_Hoch / 512);// Aktuell_az; |
179 | DebugOut.Analog[8] = KompassValue; |
179 | DebugOut.Analog[8] = KompassValue; |
180 | DebugOut.Analog[9] = UBat; |
180 | DebugOut.Analog[9] = UBat; |
181 | DebugOut.Analog[10] = SenderOkay; |
181 | DebugOut.Analog[10] = SenderOkay; |
182 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
182 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
183 | DebugOut.Analog[12] = Motor[0].SetPoint; |
183 | DebugOut.Analog[12] = Motor[0].SetPoint; |
Line 216... | Line 216... | ||
216 | // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
216 | // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
217 | ANALOG_OFF; |
217 | ANALOG_OFF; |
218 | MesswertNick = AdWertNick; |
218 | MesswertNick = AdWertNick; |
219 | MesswertRoll = AdWertRoll; |
219 | MesswertRoll = AdWertRoll; |
220 | MesswertGier = AdWertGier; |
220 | MesswertGier = AdWertGier; |
221 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
221 | Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick; |
222 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
222 | Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll; |
223 | Mittelwert_AccHoch = (long)AdWertAccHoch; |
- | |
224 | // ADC einschalten |
223 | // ADC einschalten |
225 | ANALOG_ON; |
224 | ANALOG_ON; |
226 | for(i=0;i<8;i++) |
225 | for(i=0;i<8;i++) |
227 | { |
226 | { |
228 | int tmp; |
227 | int tmp; |
Line 309... | Line 308... | ||
309 | 308 | ||
310 | MesswertNick = 0; |
309 | MesswertNick = 0; |
311 | MesswertRoll = 0; |
310 | MesswertRoll = 0; |
312 | MesswertGier = 0; |
311 | MesswertGier = 0; |
313 | Delay_ms_Mess(100); |
312 | Delay_ms_Mess(100); |
314 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
313 | Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick; |
315 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
314 | Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll; |
316 | IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
315 | IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
317 | IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
316 | IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
318 | Mess_IntegralNick2 = IntegralNick; |
317 | Mess_IntegralNick2 = IntegralNick; |
319 | Mess_IntegralRoll2 = IntegralRoll; |
318 | Mess_IntegralRoll2 = IntegralRoll; |
Line 370... | Line 369... | ||
370 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
369 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
371 | RohMesswertNick = MesswertNick; |
370 | RohMesswertNick = MesswertNick; |
372 | RohMesswertRoll = MesswertRoll; |
371 | RohMesswertRoll = MesswertRoll; |
Line 373... | Line 372... | ||
373 | 372 | ||
374 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
373 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
375 | Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L; |
374 | Mittelwert_AccNick = (Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * AdWertAccNick))) / 4L; |
376 | Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L; |
- | |
377 | Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 3 + ((long)AdWertAccHoch)) / 4L; |
375 | Mittelwert_AccRoll = (Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * AdWertAccRoll))) / 4L; |
378 | IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; |
376 | IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; |
379 | IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
377 | IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
380 | NaviAccNick += AdWertAccNick; |
378 | NaviAccNick += AdWertAccNick; |
381 | NaviAccRoll += AdWertAccRoll; |
379 | NaviAccRoll += AdWertAccRoll; |
Line 1078... | Line 1076... | ||
1078 | if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; } |
1076 | if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; } |
1079 | if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; } |
1077 | if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; } |
1080 | } |
1078 | } |
1081 | GierGyroFehler = 0; |
1079 | GierGyroFehler = 0; |
Line 1082... | Line -... | ||
1082 | - | ||
1083 | 1080 | ||
1084 | #define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2) |
1081 | #define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2) |
1085 | #define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4 |
1082 | #define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4 |
1086 | #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16 |
1083 | #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16 |
1087 | #define BEWEGUNGS_LIMIT 20000 |
1084 | #define BEWEGUNGS_LIMIT 20000 |