Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
550 | walter | 1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 10.2007 by Jochen Kromayer |
||
3 | // + es handelt sich um eine Beta-Software, die zu nichtkommerziellen Zwecken frei veröffentlich und weitergegeben werden darf. Für evtl. |
||
4 | // + Personen- und/der Sachschäden ist jegliche Haftung ausgeschlossen |
||
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
6 | #include "main.h" |
||
7 | |||
8 | //Sinusfunktion von 0° bis 359°. Ergebnis wurde mit 1024 multipliziert, damit als Definition platzsparend int |
||
9 | //verwendet werden kann. D.h., es muss also in der späteren Berechnung noch durch 1024 dividiert werden //(051207Kr) |
||
10 | const int c_sin[] = {0,18,36,54,71,89,107,125,143,160,178,195,213,230,248,265,282,299,316,333,350,367,384,400, |
||
11 | 416,433,449,465,481,496,512,527,543,558,573,587,602,616,630,644,658,672,685,698,711,724,737,749, |
||
12 | 761,773,784,796,807,818,828,839,849,859,868,878,887,896,904,912,920,928,935,943,949,956,962,968, |
||
13 | 974,979,984,989,994,998,1002,1005,1008,1011,1014,1016,1018,1020,1022,1023,1023,1024,1024,1024,1023,1023,1022,1020, |
||
14 | 1018,1016,1014,1011,1008,1005,1002,998,994,989,984,979,974,968,962,956,949,943,935,928,920,912,904,896, |
||
15 | 887,878,868,859,849,839,828,818,807,796,784,773,761,749,737,724,711,698,685,672,658,644,630,616, |
||
16 | 602,587,573,558,543,527,512,496,481,465,449,433,416,400,384,367,350,333,316,299,282,265,248,230, |
||
17 | 213,195,178,160,143,125,107,89,71,54,36,18,0,-18,-36,-54,-71,-89,-107,-125,-143,-160,-178,-195, |
||
18 | -213,-230,-248,-265,-282,-299,-316,-333,-350,-367,-384,-400,-416,-433,-449,-465,-481,-496,-512,-527,-543,-558,-573,-587, |
||
19 | -602,-616,-630,-644,-658,-672,-685,-698,-711,-724,-737,-749,-761,-773,-784,-796,-807,-818,-828,-839,-849,-859,-868,-878, |
||
20 | -887,-896,-904,-912,-920,-928,-935,-943,-949,-956,-962,-968,-974,-979,-984,-989,-994,-998,-1002,-1005,-1008,-1011,-1014,-1016, |
||
21 | -1018,-1020,-1022,-1023,-1023,-1024,-1024,-1024,-1023,-1023,-1022,-1020,-1018,-1016,-1014,-1011,-1008,-1005,-1002,-998,-994,-989,-984,-979, |
||
22 | -974,-968,-962,-956,-949,-943,-935,-928,-920,-912,-904,-896,-887,-878,-868,-859,-849,-839,-828,-818,-807,-796,-784,-773, |
||
23 | -761,-749,-737,-724,-711,-698,-685,-672,-658,-644,-630,-616,-602,-587,-573,-558,-543,-527,-512,-496,-481,-465,-449,-433, |
||
24 | -416,-400,-384,-367,-350,-333,-316,-299,-282,-265,-248,-230,-213,-195,-178,-160,-143,-125,-107,-89,-71,-54,-36,-18}; |
||
25 | |||
26 | //Cosinusfunktion von 0° bis 359°. Ergebnis wurde mit 1024 multipliziert, damit als Definition platzsparend int |
||
27 | //verwendet werden kann. D.h., es muss also in der späteren Berechnung noch durch 1024 dividiert werden //(051207Kr) |
||
28 | const int c_cos[] = {1024,1024,1023,1023,1022,1020,1018,1016,1014,1011,1008,1005,1002,998,994,989,984,979,974,968,962,956,949,943, |
||
29 | 935,928,920,912,904,896,887,878,868,859,849,839,828,818,807,796,784,773,761,749,737,724,711,698, |
||
30 | 685,672,658,644,630,616,602,587,573,558,543,527,512,496,481,465,449,433,416,400,384,367,350,333, |
||
31 | 316,299,282,265,248,230,213,195,178,160,143,125,107,89,71,54,36,18,0,-18,-36,-54,-71,-89, |
||
32 | -107,-125,-143,-160,-178,-195,-213,-230,-248,-265,-282,-299,-316,-333,-350,-367,-384,-400,-416,-433,-449,-465,-481,-496, |
||
33 | -512,-527,-543,-558,-573,-587,-602,-616,-630,-644,-658,-672,-685,-698,-711,-724,-737,-749,-761,-773,-784,-796,-807,-818, |
||
34 | -828,-839,-849,-859,-868,-878,-887,-896,-904,-912,-920,-928,-935,-943,-949,-956,-962,-968,-974,-979,-984,-989,-994,-998, |
||
35 | -1002,-1005,-1008,-1011,-1014,-1016,-1018,-1020,-1022,-1023,-1023,-1024,-1024,-1024,-1023,-1023,-1022,-1020,-1018,-1016,-1014,-1011,-1008,-1005, |
||
36 | -1002,-998,-994,-989,-984,-979,-974,-968,-962,-956,-949,-943,-935,-928,-920,-912,-904,-896,-887,-878,-868,-859,-849,-839, |
||
37 | -828,-818,-807,-796,-784,-773,-761,-749,-737,-724,-711,-698,-685,-672,-658,-644,-630,-616,-602,-587,-573,-558,-543,-527, |
||
38 | -512,-496,-481,-465,-449,-433,-416,-400,-384,-367,-350,-333,-316,-299,-282,-265,-248,-230,-213,-195,-178,-160,-143,-125, |
||
39 | -107,-89,-71,-54,-36,-18,0,18,36,54,71,89,107,125,143,160,178,195,213,230,248,265,282,299, |
||
40 | 316,333,350,367,384,400,416,433,449,465,481,496,512,527,543,558,573,587,602,616,630,644,658,672, |
||
41 | 685,698,711,724,737,749,761,773,784,796,807,818,828,839,849,859,868,878,887,896,904,912,920,928, |
||
42 | 935,943,949,956,962,968,974,979,984,989,994,998,1002,1005,1008,1011,1014,1016,1018,1020,1022,1023,1023,1024}; |
||
43 | |||
44 | |||
45 | |||
46 | long Soll_Position_North = 0; |
||
47 | long Soll_Position_East = 0; |
||
48 | |||
49 | long GPS_Positionsabweichung_North = 0; |
||
50 | long GPS_Positionsabweichung_East = 0; |
||
51 | long GPS_Geschwindigkeit_North = 0; |
||
52 | long GPS_Geschwindigkeit_East = 0; |
||
53 | signed int GPS_StickSense = 15; |
||
54 | long P_Einfluss_North = 0; |
||
55 | long D_Einfluss_North = 0; |
||
56 | long P_Einfluss_East = 0; |
||
57 | long D_Einfluss_East = 0; |
||
58 | |||
59 | signed int GPS_North = 0; |
||
60 | signed int GPS_East = 0; |
||
61 | signed int GPS_Nick = 0; |
||
62 | signed int GPS_Roll = 0; |
||
63 | |||
64 | volatile char gethome = 0; //Flag ob GPS_Home_Position gelernt ist //(280807Kr) |
||
65 | long GPS_Home_North = 0; |
||
66 | long GPS_Home_East = 0; |
||
67 | |||
68 | unsigned char blinkcount_LED2 = 0; //Hilfszähler für die blinkende LED (010907Kr) |
||
69 | |||
70 | #define GPS_Limit 35 //(031207Kr) |
||
71 | #define Limit_D_Anteil 30 //(031207Kr) |
||
72 | |||
73 | |||
74 | |||
75 | //**************************** MIRCOS DEFINITIONSANFANG FÜR DIE FILTERUNG |
||
76 | // Filterung der GPS Messwerte vom Ublox-Empfänger. Es wird der gleitende Durchschnitt aus n Messwerten gebildet |
||
77 | |||
78 | #define MITTELWERTZAHL 8 // Anzahl der Messwerte fuer gleitenden Mittelwert |
||
79 | volatile char NeuerMittelwert = 0; //Flag ob ein neuer Mittelwert vorliegt, der mit GPS Frequenz berechnet wird, da nur dann neue Messwerte vom GPS vorliegen |
||
80 | volatile unsigned char FilterPos = 0; |
||
81 | volatile long NORTH_MITTEL = 0; |
||
82 | volatile long EAST_MITTEL = 0; |
||
83 | volatile long long NORTH_LONG_LONG = 0; |
||
84 | volatile long long EAST_LONG_LONG = 0; |
||
85 | volatile int filterfilled = 0; |
||
86 | |||
87 | typedef struct { |
||
88 | |||
89 | long NORTH[MITTELWERTZAHL]; |
||
90 | |||
91 | } FILTER_NORTH; // Filterdefinition für die Positions-Nordwerte aus dem GPS-Empfänger |
||
92 | |||
93 | typedef struct { |
||
94 | |||
95 | long EAST[MITTELWERTZAHL]; |
||
96 | |||
97 | } FILTER_EAST; // Filterdefinition für die Positions-Ostwerte aus dem GPS-Empfänger |
||
98 | |||
99 | FILTER_NORTH filter_north; |
||
100 | FILTER_EAST filter_east; |
||
101 | |||
102 | //**************************** MIRCOS DEFINITIONSENDE FÜR DIE FILTERUNG |
||
103 | |||
104 | |||
105 | void gps_main(void) |
||
106 | { |
||
107 | |||
108 | //**************************** MIRCOS FILTERUNG ANFANG |
||
109 | //bilde neuen Positionsmittelwert, wenn 3D-Fix und neue GPS-Daten vorliegen |
||
110 | // if(actualPos.GPSFix == 3 && newData_navPosUtm == 1) |
||
111 | if(actualPos.GPSFix == 3) // hier umkommentieren, wenn dauernd (also nicht nur mit der GPS-Updaterate) die GPS-Berechnung durchgeführt werden soll |
||
112 | |||
113 | { |
||
114 | |||
115 | filter_north.NORTH[FilterPos] = actualPos.northing; |
||
116 | filter_east.EAST[FilterPos] = actualPos.easting; |
||
117 | FilterPos++; |
||
118 | if (FilterPos==MITTELWERTZAHL) FilterPos=0; |
||
119 | |||
120 | if (filterfilled<MITTELWERTZAHL) filterfilled++; |
||
121 | |||
122 | NORTH_LONG_LONG = 0; |
||
123 | EAST_LONG_LONG = 0; |
||
124 | for (int i=0; i<filterfilled; i++) |
||
125 | { |
||
126 | NORTH_LONG_LONG += filter_north.NORTH[i]; |
||
127 | EAST_LONG_LONG += filter_east.EAST[i]; |
||
128 | } |
||
129 | NORTH_LONG_LONG /= filterfilled; |
||
130 | EAST_LONG_LONG /= filterfilled; |
||
131 | |||
132 | NORTH_MITTEL = NORTH_LONG_LONG; |
||
133 | EAST_MITTEL = EAST_LONG_LONG; |
||
134 | |||
135 | NeuerMittelwert = 1; |
||
136 | newData_navPosUtm = 0; |
||
137 | } |
||
138 | //******************************* MIRCOS FILTERUNG ENDE |
||
139 | |||
140 | |||
141 | |||
142 | |||
143 | //wenn kein 3D-Fix vorhanden, über das Poti jedoch irgendeine GPS Funktion aktiv ist, werden die GPS-Steuerbefehle |
||
144 | //auf Null gesetzt, um unkontrolliertes Wegfliegen durch das GPS zu verhindern. Dadurch wird abgesichert |
||
145 | //(auch im laufenden Flug), dass bei einem Verlust der GPS-Verbindung GPS deaktivert wird |
||
146 | if (actualPos.GPSFix < 3 && Poti3 > 70) |
||
147 | { |
||
148 | GPS_Nick = 0; |
||
149 | GPS_Roll = 0; |
||
150 | LED2_OFF; |
||
151 | beeptime = 50; |
||
152 | } |
||
153 | |||
154 | |||
155 | |||
156 | // speichert die GPS_Home_Position beim 3D-Fix, MotorEINschalten und wenn noch nicht zu weit weggeflogen wurde |
||
157 | // (modell_fliegt_gps < 2000) einmalig ab. Die Flugzeitabfrage sichert ab, dass bei einem Losfliegen ohne 3D-Fix die Home_Positinen |
||
158 | // irgendwann später dort gelernt werden, wo dann zu ersten mal ein 3D-Fix vorhanden ist |
||
159 | if (actualPos.GPSFix == 3 && MotorenEin == 1 && gethome == 0 && modell_fliegt_gps < 2000) |
||
160 | { |
||
161 | GPS_Home_North = NORTH_MITTEL; |
||
162 | GPS_Home_East = EAST_MITTEL; |
||
163 | // Muss hier auch eingelernt werden, damit falls das Poti3 beim Motorenstarten in |
||
164 | // Mittelstellung sein sollte, trotzdem schon eine Sollposition vorliegt. Andernfalls würde |
||
165 | // GPS_Nick und _Roll maximal werden bis zum ersten Mal Stick_Nick und _Roll gesteuert würde |
||
166 | Soll_Position_North = NORTH_MITTEL; |
||
167 | Soll_Position_East = EAST_MITTEL; |
||
168 | beeptime = 50; |
||
169 | gethome = 1; |
||
170 | } |
||
171 | |||
172 | |||
173 | |||
174 | |||
175 | //löscht die GPS_Home_Position beim MotorAUSschalten wieder und resetet das Flag gethome //(280807Kr) |
||
176 | if (MotorenEin == 0) |
||
177 | { |
||
178 | GPS_Home_North = 0; |
||
179 | GPS_Home_East = 0; |
||
180 | gethome = 0; |
||
181 | modell_fliegt_gps = 0; |
||
182 | } |
||
183 | |||
184 | |||
185 | |||
186 | |||
187 | // wenn Schalter ausgeschaltet, dann wird die LED2 ausgeschaltet und das GPS Steuerkommando gelöscht |
||
188 | if(Poti3 < 70) |
||
189 | { |
||
190 | // Sollpositionen müssen auch hier beschrieben werden, damit ausgeschlossen ist, dass falls gelandet wird und die GPS-Funktion |
||
191 | // über Poti3 deaktivert und anschließend wieder aktiviert wird keine Soll-Positionen vorliegen. |
||
192 | Soll_Position_North = NORTH_MITTEL; |
||
193 | Soll_Position_East = EAST_MITTEL; |
||
194 | |||
195 | GPS_Nick = 0; |
||
196 | GPS_Roll = 0; |
||
197 | LED2_OFF; |
||
198 | //hierdurch soll verhindert werden, dass wenn nach schonmal aktiver GPS Funktion (und damit 3D-fix) der GPS-Empfänger |
||
199 | //abgezogen werden sollte weiterhin ein 3D-fix angezeigt wird. Grund ist, dass in diesem Fall in der UART.c nach einem |
||
200 | //Abziehen der Status nicht mehr auf Null zurückgesetzt wird. Dies geschied nun beim Ausschalten der GPS-Funktion. |
||
201 | //Sie ist dann ohne erneuten 3D-Fix nicht mehr aktivierbar. (111107Kr) |
||
202 | actualPos.GPSFix = 0; |
||
203 | } |
||
204 | |||
205 | |||
206 | // wenn 3D-Fix, Schalter im mittleren Bereich und vom Pilot gesteuert wird, wird ständig neue GPS-Sollposition eingelernt |
||
207 | if(actualPos.GPSFix == 3 && Poti3 > 70 && Poti3 < 150 && (StickNick > GPS_StickSense || StickNick < -GPS_StickSense || StickRoll > GPS_StickSense || StickRoll < -GPS_StickSense)) |
||
208 | { |
||
209 | GPS_Nick = 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert |
||
210 | GPS_Roll = 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert |
||
211 | |||
212 | Soll_Position_North = NORTH_MITTEL; // Soll-Position wird geschrieben |
||
213 | Soll_Position_East = EAST_MITTEL; // Soll-Position wird geschrieben |
||
214 | |||
215 | //++++++++++++++++++++++++++++++++++++ |
||
216 | //Sorgt lediglich dafür, dass die LED2 langsam blinkt. Dauer ist durch die "> x" Abfrage festgelegt |
||
217 | //++++++++++++++++++++++++++++++++++++ |
||
218 | if(blinkcount_LED2 >= 200) // maximal erlaubt 255 wegen unsigned char |
||
219 | { |
||
220 | LED2_FLASH; |
||
221 | blinkcount_LED2 = 0; |
||
222 | } |
||
223 | blinkcount_LED2++; |
||
224 | //++++++++++++++++++++++++++++++++++++ |
||
225 | } |
||
226 | |||
227 | |||
228 | // wenn 3D-Fix, Schalter im mittleren Bereich (oder höher), neuer Mittelwert vorhanden, wenigstens einmal Sollposition gelernt und der Pilot nicht selbst steuert wird eine GPS-Funktion aktiviert |
||
229 | if (actualPos.GPSFix == 3 && Poti3 > 70 && NeuerMittelwert == 1 && Soll_Position_North != 0 && Soll_Position_East != 0 && StickNick <= GPS_StickSense && StickNick >= -GPS_StickSense && StickRoll <= GPS_StickSense && StickRoll >= -GPS_StickSense) |
||
230 | { |
||
231 | //Berechnung der GPS-Positionsabweichung je nach aktivierter GPS-Funktion (GPS_HOLD oder HOMING) |
||
232 | |||
233 | //DYNAMISCHES GPS-HOLD, wenn Schalter noch in Mittelstellung ist wird die zuletzt gelernte Soll_Position angeflogen |
||
234 | if(Poti3 < 150) |
||
235 | { |
||
236 | GPS_Positionsabweichung_North = (Soll_Position_North - NORTH_MITTEL); |
||
237 | GPS_Positionsabweichung_East = (Soll_Position_East - EAST_MITTEL); |
||
238 | LED2_ON; |
||
239 | } |
||
240 | |||
241 | //andernfalls, wenn Schalter in max. Stellung und zusätzlich die GPS_Home_Positionen beim Motorenstarten gelernt |
||
242 | //werden konnten, wird zur GPS_Home_Position geflogen |
||
243 | if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0) |
||
244 | { |
||
245 | GPS_Positionsabweichung_North = (GPS_Home_North - NORTH_MITTEL); |
||
246 | GPS_Positionsabweichung_East = (GPS_Home_East - EAST_MITTEL); |
||
247 | // durch das ständige Neulernen der Soll_Positionen während HOMING wird erreicht, dass der MK nach dem Umschalten |
||
248 | // von HOMING zurück zu GPS-HOLD nicht wieder an die alte Soll_Position zurück fliegt |
||
249 | Soll_Position_North = NORTH_MITTEL; |
||
250 | Soll_Position_East = EAST_MITTEL; |
||
251 | LED2_ON; |
||
252 | } |
||
253 | |||
254 | //verhindert, dass beim Umschalten von Mittelstellung (GPS_HOLD) auf Maximalstellung (HOMING) OHNE gelernte Homeposition |
||
255 | //die GPS Funktion unkontrolliert steuert. Daher wird GPS_HOLD aktiv gehalten, die Sollpositon wird bei Stickbefehlen |
||
256 | //jedoch nicht wie beim dynamischen GPS_HOLD überschrieben. Zusätzlich wird der Summer eingeschaltet. |
||
257 | if (Poti3 >= 150 && GPS_Home_North == 0 && GPS_Home_East == 0) |
||
258 | { |
||
259 | GPS_Positionsabweichung_North = (Soll_Position_North - NORTH_MITTEL); |
||
260 | GPS_Positionsabweichung_East = (Soll_Position_East - EAST_MITTEL); |
||
261 | LED2_ON; |
||
262 | beeptime = 50; |
||
263 | } |
||
264 | |||
265 | //GPS-Geschwindigkeiten |
||
266 | GPS_Geschwindigkeit_North = actualPos.velNorth; //actualPos.velNorth ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Nord-Süd Richtung |
||
267 | GPS_Geschwindigkeit_East = actualPos.velEast; //actualPos.velEast ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Ost-West Richtung |
||
268 | |||
269 | |||
270 | //Berechnung der Einzelkomponenten des PD-Reglers |
||
271 | P_Einfluss_North = (P_GPS_Verstaerkung * GPS_Positionsabweichung_North) / 2048; |
||
272 | D_Einfluss_North = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_North) / 256; |
||
273 | |||
274 | P_Einfluss_East = (P_GPS_Verstaerkung * GPS_Positionsabweichung_East) / 2048; |
||
275 | D_Einfluss_East = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_East) / 256; |
||
276 | |||
277 | |||
278 | // Begrenzung des maximalen D-Anteils für pos. und neg. Werte. Grenze muss so geählt werden, |
||
279 | // dass das Limit im normalen Position-Hold nicht erreicht wird und somit keinen Einfluss hat. |
||
280 | // Grund: Beim Zufliegen auf weiter entfernte Ziele bremst der D-Anteil kurz vor dem Ziel den Mikrokopter sehr stark ab. |
||
281 | // Dieses Limit wirkt nur, wenn sich der MK im HOMING Modus befindet und sich noch weit (>5m) von der Home-Position weg befindet. |
||
282 | if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0 && (abs(GPS_Positionsabweichung_North) > 500 || abs(GPS_Positionsabweichung_East) > 500)) |
||
283 | { |
||
284 | if (D_Einfluss_North > Limit_D_Anteil) D_Einfluss_North = Limit_D_Anteil; |
||
285 | if (D_Einfluss_East > Limit_D_Anteil) D_Einfluss_East = Limit_D_Anteil; |
||
286 | if (D_Einfluss_North < -1 * Limit_D_Anteil) D_Einfluss_North = -1 * Limit_D_Anteil; |
||
287 | if (D_Einfluss_East < -1 * Limit_D_Anteil) D_Einfluss_East = -1 * Limit_D_Anteil; |
||
288 | } |
||
289 | |||
290 | // PD-Regler |
||
291 | GPS_North = (-P_Einfluss_North + D_Einfluss_North); |
||
292 | GPS_East = (P_Einfluss_East - D_Einfluss_East); |
||
293 | |||
294 | //Umrechnen vom globalen North- bzw. East- in das körperfeste X- bzw. Y- Koordinatensystem |
||
295 | GPS_Nick = ((c_cos[KompassValue]*GPS_North)/1024 - (c_sin[KompassValue]*GPS_East)/1024); |
||
296 | GPS_Roll = ((c_cos[KompassValue]*GPS_East)/1024 + (c_sin[KompassValue]*GPS_North)/1024); |
||
297 | |||
298 | // Begrenzung des maximalen GPS Einflusses für positive und negative Werte |
||
299 | if (GPS_Nick > GPS_Limit) GPS_Nick = GPS_Limit; |
||
300 | if (GPS_Roll > GPS_Limit) GPS_Roll = GPS_Limit; |
||
301 | if (GPS_Nick < -1 * GPS_Limit) GPS_Nick = -1 * GPS_Limit; |
||
302 | if (GPS_Roll < -1 * GPS_Limit) GPS_Roll = -1 * GPS_Limit; |
||
303 | |||
304 | //Funktion wird dadurch nur alle 250 ms aufgerufen, wenn neue Mittelwerte aus den GPS-Daten vorliegen. |
||
305 | //FUNKTIONIERT MOMENTAN NOCH NICHT, DA STÄNDIG OHNE UNTERBRECHUNG NEUE MITTELWERTE BERECHNET WERDEN. |
||
306 | //GRUND: DIE ABFRAGE NACH NEUE GPS-DATEN ERFOLGT NOCH NICHT |
||
307 | NeuerMittelwert = 0; |
||
308 | } |
||
309 | } |
||
310 |