Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
836 | MikeW | 1 | /* |
2 | Copyright 2008, by Michael Walter |
||
3 | |||
4 | All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
||
5 | General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
||
6 | WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||
7 | See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
||
8 | License along with this program. If not, see <http://www.gnu.org/licenses/>. |
||
9 | |||
10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
||
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
||
12 | unless it is stated otherwise. |
||
13 | */ |
||
14 | |||
15 | #include "main.h" |
||
16 | #include "kafi.h" |
||
17 | |||
18 | #include "mymath.h" |
||
19 | #include <math.h> |
||
20 | |||
21 | int GPSTracking = 0; |
||
22 | int targetPosValid = 0; |
||
23 | int homePosValid = 0; |
||
24 | |||
25 | uint8_t gpsState; |
||
26 | |||
27 | gpsInfo_t actualPos; // measured position (last gps record) |
||
28 | gpsInfo_t targetPos; // measured position (last gps record) |
||
29 | gpsInfo_t homePos; // measured position (last gps record) |
||
30 | |||
31 | NAV_STATUS_t navStatus; |
||
32 | NAV_POSLLH_t navPosLlh; |
||
33 | NAV_POSUTM_t navPosUtm; |
||
34 | NAV_VELNED_t navVelNed; |
||
35 | |||
36 | signed int GPS_Nick = 0; |
||
37 | signed int GPS_Roll = 0; |
||
38 | |||
39 | long distanceNS = 0; |
||
40 | long distanceEW = 0; |
||
41 | long velocityNS = 0; |
||
42 | long velocityEW = 0; |
||
43 | unsigned long maxDistance = 0; |
||
44 | |||
45 | int roll_gain = 0; |
||
46 | int nick_gain = 0; |
||
47 | int nick_gain_p = 0; |
||
48 | int nick_gain_d = 0; |
||
49 | int roll_gain_p = 0; |
||
50 | int roll_gain_d = 0; |
||
51 | int Override = 0; |
||
52 | int TargetGier = 0; |
||
53 | |||
54 | extern int sollGier; |
||
55 | |||
56 | char *ubxP, *ubxEp, *ubxSp; // pointers to packet currently transfered |
||
57 | uint8_t CK_A, CK_B; // UBX checksum bytes |
||
58 | uint8_t ignorePacket; // true when previous packet was not processed |
||
59 | unsigned short msgLen; |
||
60 | uint8_t msgID; |
||
61 | |||
62 | void GPSscanData (void); |
||
63 | void GPSupdate(void); |
||
64 | |||
65 | |||
66 | /* **************************************************************************** |
||
67 | Functionname: GPSupdate */ /*! |
||
68 | Description: |
||
69 | |||
70 | @return void |
||
71 | @pre - |
||
72 | @post - |
||
73 | @author Michael Walter |
||
74 | **************************************************************************** */ |
||
75 | void GPSupdate(void) |
||
76 | { |
||
77 | float SIN_H, COS_H; |
||
78 | long max_p = 0; |
||
79 | long max_d = 0; |
||
80 | |||
81 | if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 15 || |
||
82 | abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 || |
||
83 | abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 15) |
||
84 | { |
||
85 | Override = 1; |
||
86 | } |
||
87 | else |
||
88 | { |
||
89 | Override = 0; |
||
90 | } |
||
91 | |||
92 | /* Set Home Position */ |
||
93 | if ((actualPos.state > 2) && |
||
94 | (homePosValid == 0)) |
||
95 | { |
||
96 | /* Save Current Position */ |
||
97 | homePos.north = actualPos.north; |
||
98 | homePos.east = actualPos.east; |
||
99 | homePos.altitude = actualPos.altitude ; |
||
100 | homePosValid = 1; |
||
101 | } |
||
102 | |||
103 | /* Set Target Position */ |
||
104 | if ((actualPos.state > 2) && |
||
105 | (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100)) |
||
106 | { |
||
107 | /* Save Current Position */ |
||
108 | targetPos.north = actualPos.north; |
||
109 | targetPos.east = actualPos.east; |
||
110 | targetPos.altitude = actualPos.altitude ; |
||
111 | targetPosValid = 1; |
||
112 | |||
113 | if(BeepMuster == 0xffff) |
||
114 | { |
||
115 | beeptime = 5000; |
||
116 | BeepMuster = 0x0100; |
||
117 | } |
||
118 | } |
||
119 | |||
120 | if ((GPSTracking == 1) && |
||
121 | (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < 100)) |
||
122 | { |
||
123 | GPSTracking = 0; |
||
124 | beeptime = 5000; |
||
125 | BeepMuster = 0x0080; |
||
126 | } |
||
127 | |||
128 | /* The System is in Tracking State*/ |
||
129 | if ((GPSTracking == 0) && |
||
130 | (targetPosValid == 1) && |
||
131 | (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100) && |
||
132 | (actualPos.state > 2)) |
||
133 | { |
||
134 | GPSTracking = 1; |
||
135 | beeptime = 5000; |
||
136 | BeepMuster = 0x0c00; |
||
137 | } |
||
138 | |||
139 | max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20); |
||
140 | max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40); |
||
141 | |||
142 | #if 0 |
||
143 | DebugOut.Analog[11] = max_p; |
||
144 | DebugOut.Analog[12] = max_d; |
||
145 | #endif |
||
146 | |||
147 | static int GPSTrackingCycles = 0; |
||
148 | long maxGainPos = 140; |
||
149 | long maxGainVel = 140; |
||
150 | |||
151 | /* Slowly ramp up gain */ |
||
152 | if (GPSTrackingCycles < 1000) |
||
153 | { |
||
154 | GPSTrackingCycles++; |
||
155 | } |
||
156 | maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000; |
||
157 | maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000; |
||
158 | |||
159 | if (actualPos.state > 2 ) |
||
160 | { |
||
161 | distanceNS = actualPos.north - targetPos.north; // in 0.1m |
||
162 | distanceEW = actualPos.east - targetPos.east; // in 0.1m |
||
163 | velocityNS = actualPos.velNorth; // in 0.1m/s |
||
164 | velocityEW = actualPos.velEast; // in 0.1m/s |
||
165 | maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
||
166 | |||
167 | // Limit GPS_Nick to 25 |
||
168 | nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50))); //m |
||
169 | nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velNorth * max_d)/50))); //m/s |
||
170 | roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m |
||
171 | roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velEast * max_d)/50))); //m/s |
||
172 | |||
173 | TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10; |
||
174 | } |
||
175 | |||
176 | if ((GPSTracking == 1) && |
||
177 | (actualPos.state > 2) && |
||
178 | (Override == 0)) |
||
179 | { |
||
180 | /* Calculate Distance to Target */ |
||
181 | SIN_H = (float) sin(status.Psi); |
||
182 | COS_H =(float) cos(status.Psi); |
||
183 | |||
184 | /* PD-Regler */ |
||
185 | nick_gain = nick_gain_p + nick_gain_d; |
||
186 | roll_gain = -(roll_gain_p + roll_gain_d); |
||
187 | |||
188 | GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain); |
||
189 | GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain); |
||
190 | |||
191 | GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick)); |
||
192 | GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll)); |
||
193 | |||
194 | /* Turn the mikrokopter slowly towards the target position */ |
||
195 | { |
||
196 | int OffsetGier; |
||
197 | if (maxDistance / 10 > 2) |
||
198 | { |
||
199 | OffsetGier = 2; |
||
200 | } |
||
201 | else |
||
202 | { |
||
203 | OffsetGier = 0; |
||
204 | } |
||
205 | |||
206 | if (TargetGier < 0) |
||
207 | { |
||
208 | TargetGier += 3600; |
||
209 | } |
||
210 | if (TargetGier > sollGier) |
||
211 | { |
||
212 | if (TargetGier - sollGier < 1800) |
||
213 | { |
||
214 | sollGier += OffsetGier; |
||
215 | } |
||
216 | else |
||
217 | { |
||
218 | sollGier -= OffsetGier; |
||
219 | } |
||
220 | } |
||
221 | else |
||
222 | { |
||
223 | if (sollGier - TargetGier < 1800) |
||
224 | { |
||
225 | sollGier -= OffsetGier; |
||
226 | } |
||
227 | else |
||
228 | { |
||
229 | sollGier += OffsetGier; |
||
230 | } |
||
231 | } |
||
232 | } |
||
233 | #if 0 |
||
234 | /* Go round in a square */ |
||
235 | if (maxDistance / 10 < 10) |
||
236 | { |
||
237 | static int iState = 0; |
||
238 | switch (iState) |
||
239 | { |
||
240 | case 0: |
||
241 | targetPos.north += 400; |
||
242 | targetPos.east += 0; |
||
243 | GPSTrackingCycles = 0; |
||
244 | iState++; |
||
245 | break; |
||
246 | case 1: |
||
247 | targetPos.north += 0; |
||
248 | targetPos.east += 400; |
||
249 | GPSTrackingCycles = 0; |
||
250 | iState++; |
||
251 | break; |
||
252 | case 2: |
||
253 | targetPos.north -= 400; |
||
254 | targetPos.east += 0; |
||
255 | GPSTrackingCycles = 0; |
||
256 | iState++; |
||
257 | break; |
||
258 | case 3: |
||
259 | targetPos.north += 0; |
||
260 | targetPos.east -= 400; |
||
261 | GPSTrackingCycles = 0; |
||
262 | iState=0; |
||
263 | break; |
||
264 | default: |
||
265 | iState=0; |
||
266 | break; |
||
267 | } |
||
268 | beeptime = 5000; |
||
269 | BeepMuster = 0x0c00; |
||
270 | } |
||
271 | #endif |
||
272 | } |
||
273 | else |
||
274 | { |
||
275 | GPS_Nick = 0; |
||
276 | GPS_Roll = 0; |
||
277 | GPSTrackingCycles = 0; |
||
278 | } |
||
279 | } |
||
280 | |||
281 | /* **************************************************************************** |
||
282 | Functionname: InitGPS */ /*! |
||
283 | Description: |
||
284 | |||
285 | @return void |
||
286 | @pre - |
||
287 | @post - |
||
288 | @author |
||
289 | **************************************************************************** */ |
||
290 | void InitGPS(void) |
||
291 | { |
||
292 | // USART1 Control and Status Register A, B, C and baud rate register |
||
293 | uint8_t sreg = SREG; |
||
294 | //uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1); |
||
295 | |||
296 | // disable all interrupts before reconfiguration |
||
297 | cli(); |
||
298 | |||
299 | // disable RX-Interrupt |
||
300 | UCSR1B &= ~(1 << RXCIE1); |
||
301 | // disable TX-Interrupt |
||
302 | UCSR1B &= ~(1 << TXCIE1); |
||
303 | // disable DRE-Interrupt |
||
304 | UCSR1B &= ~(1 << UDRIE1); |
||
305 | |||
306 | // set direction of RXD1 and TXD1 pins |
||
307 | // set RXD1 (PD2) as an input pin |
||
308 | PORTD |= (1 << PORTD2); |
||
309 | DDRD &= ~(1 << DDD2); |
||
310 | |||
311 | // set TXD1 (PD3) as an output pin |
||
312 | PORTD |= (1 << PORTD3); |
||
313 | DDRD |= (1 << DDD3); |
||
314 | |||
315 | // USART0 Baud Rate Register |
||
316 | // set clock divider |
||
317 | UBRR1H = 0; |
||
318 | UBRR1L = BAUDRATE; |
||
319 | |||
320 | // enable double speed operation |
||
321 | //UCSR1A |= (1 << U2X1); |
||
322 | UCSR1A = _B0(U2X1); |
||
323 | |||
324 | // enable receiver and transmitter |
||
325 | UCSR1B = (1 << TXEN1) | (1 << RXEN1); |
||
326 | // set asynchronous mode |
||
327 | UCSR1C &= ~(1 << UMSEL11); |
||
328 | UCSR1C &= ~(1 << UMSEL10); |
||
329 | // no parity |
||
330 | UCSR1C &= ~(1 << UPM11); |
||
331 | UCSR1C &= ~(1 << UPM10); |
||
332 | // 1 stop bit |
||
333 | UCSR1C &= ~(1 << USBS1); |
||
334 | // 8-bit |
||
335 | UCSR1B &= ~(1 << UCSZ12); |
||
336 | UCSR1C |= (1 << UCSZ11); |
||
337 | UCSR1C |= (1 << UCSZ10); |
||
338 | |||
339 | // flush receive buffer explicit |
||
340 | while ( UCSR1A & (1<<RXC1) ) UDR1; |
||
341 | |||
342 | // enable interrupts at the end |
||
343 | // enable RX-Interrupt |
||
344 | UCSR1B |= (1 << RXCIE1); |
||
345 | // enable TX-Interrupt |
||
346 | UCSR1B |= (1 << TXCIE1); |
||
347 | // enable DRE interrupt |
||
348 | //UCSR1B |= (1 << UDRIE1); |
||
349 | |||
350 | // restore global interrupt flags |
||
351 | SREG = sreg; |
||
352 | |||
353 | gpsState = GPS_EMPTY; |
||
354 | } |
||
355 | |||
356 | |||
357 | /* **************************************************************************** |
||
358 | Functionname: InitGPS */ /*! |
||
359 | Description: Copy GPS paket data to actualPos |
||
360 | |||
361 | @return void |
||
362 | @pre - |
||
363 | @post - |
||
364 | @author |
||
365 | **************************************************************************** */ |
||
366 | void GPSscanData (void) |
||
367 | { |
||
368 | if (navStatus.packetStatus == 1) // valid packet |
||
369 | { |
||
370 | actualPos.state = navStatus.GPSfix; |
||
371 | if ((actualPos.state > 1) && (GPSTracking == 0)) |
||
372 | { |
||
373 | GRN_FLASH; |
||
374 | } |
||
375 | navStatus.packetStatus = 0; |
||
376 | } |
||
377 | |||
378 | if (navPosUtm.packetStatus == 1) // valid packet |
||
379 | { |
||
380 | actualPos.north = navPosUtm.NORTH/10L; |
||
381 | actualPos.east = navPosUtm.EAST/10L; |
||
382 | actualPos.altitude = navPosUtm.ALT/100; |
||
383 | actualPos.ITOW = navPosUtm.ITOW; |
||
384 | navPosUtm.packetStatus = 0; |
||
385 | } |
||
386 | /* |
||
387 | if (navPosLlh.packetStatus == 1) |
||
388 | { |
||
389 | actualPos.longi = navPosLlh.LON; |
||
390 | actualPos.lati = navPosLlh.LAT; |
||
391 | actualPos.height = navPosLlh.HEIGHT; |
||
392 | navPosLlh.packetStatus = 0; |
||
393 | } |
||
394 | */ |
||
395 | if (navVelNed.packetStatus == 1) |
||
396 | { |
||
397 | actualPos.velNorth = navVelNed.VEL_N; |
||
398 | actualPos.velEast = navVelNed.VEL_E; |
||
399 | actualPos.velDown = navVelNed.VEL_D; |
||
400 | actualPos.groundSpeed = navVelNed.GSpeed; |
||
401 | navVelNed.packetStatus = 0; |
||
402 | } |
||
403 | } |
||
404 | |||
405 | /* **************************************************************************** |
||
406 | Functionname: SIGNAL */ /*! |
||
407 | Description: |
||
408 | |||
409 | @return void |
||
410 | @pre - |
||
411 | @post - |
||
412 | @author |
||
413 | **************************************************************************** */ |
||
414 | SIGNAL (SIG_USART1_RECV) |
||
415 | { |
||
416 | uint8_t c; |
||
417 | uint8_t re; |
||
418 | |||
419 | re = (UCSR1A & (_B1(FE1) | _B1(DOR1))); // any error occured ? |
||
420 | c = UDR1; |
||
421 | |||
422 | if (re == 0) |
||
423 | { |
||
424 | switch (gpsState) |
||
425 | { |
||
426 | case GPS_EMPTY: |
||
427 | if (c == SYNC_CHAR1) |
||
428 | { |
||
429 | gpsState = GPS_SYNC1; |
||
430 | } |
||
431 | break; |
||
432 | case GPS_SYNC1: |
||
433 | if (c == SYNC_CHAR2) |
||
434 | { |
||
435 | gpsState = GPS_SYNC2; |
||
436 | } |
||
437 | else if (c != SYNC_CHAR1) |
||
438 | { |
||
439 | gpsState = GPS_EMPTY; |
||
440 | } |
||
441 | break; |
||
442 | case GPS_SYNC2: |
||
443 | if (c == CLASS_NAV) |
||
444 | { |
||
445 | gpsState = GPS_CLASS; |
||
446 | } |
||
447 | else |
||
448 | { |
||
449 | gpsState = GPS_EMPTY; |
||
450 | } |
||
451 | break; |
||
452 | case GPS_CLASS: // msg ID seen: init packed receive |
||
453 | msgID = c; |
||
454 | CK_A = CLASS_NAV + c; |
||
455 | CK_B = CLASS_NAV + CK_A; |
||
456 | gpsState = GPS_LEN1; |
||
457 | |||
458 | switch (msgID) |
||
459 | { |
||
460 | case MSGID_STATUS: |
||
461 | ubxP = (char*)&navStatus; |
||
462 | ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t)); |
||
463 | ubxSp = (char*)&navStatus.packetStatus; |
||
464 | ignorePacket = navStatus.packetStatus; |
||
465 | break; |
||
466 | /* |
||
467 | case MSGID_POSLLH: |
||
468 | ubxP = (char*)&navPosLlh; |
||
469 | ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t)); |
||
470 | ubxSp = (char*)&navPosLlh.packetStatus; |
||
471 | ignorePacket = navPosLlh.packetStatus; |
||
472 | break; |
||
473 | */ |
||
474 | case MSGID_POSUTM: |
||
475 | ubxP = (char*)&navPosUtm; |
||
476 | ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t)); |
||
477 | ubxSp = (char*)&navPosUtm.packetStatus; |
||
478 | ignorePacket = navPosUtm.packetStatus; |
||
479 | break; |
||
480 | case MSGID_VELNED: |
||
481 | ubxP = (char*)&navVelNed; |
||
482 | ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t)); |
||
483 | ubxSp = (char*)&navVelNed.packetStatus; |
||
484 | ignorePacket = navVelNed.packetStatus; |
||
485 | break; |
||
486 | default: |
||
487 | ignorePacket = 1; |
||
488 | ubxSp = (char*)0; |
||
489 | } |
||
490 | break; |
||
491 | case GPS_LEN1: // first len byte |
||
492 | msgLen = c; |
||
493 | CK_A += c; |
||
494 | CK_B += CK_A; |
||
495 | gpsState = GPS_LEN2; |
||
496 | break; |
||
497 | case GPS_LEN2: // second len byte |
||
498 | msgLen = msgLen + (c * 256); |
||
499 | CK_A += c; |
||
500 | CK_B += CK_A; |
||
501 | gpsState = GPS_FILLING; // next data will be stored in packet struct |
||
502 | break; |
||
503 | case GPS_FILLING: |
||
504 | CK_A += c; |
||
505 | CK_B += CK_A; |
||
506 | |||
507 | if ( !ignorePacket && ubxP < ubxEp) |
||
508 | { |
||
509 | *ubxP++ = c; |
||
510 | } |
||
511 | |||
512 | if (--msgLen == 0) |
||
513 | { |
||
514 | gpsState = GPS_CKA; |
||
515 | } |
||
516 | break; |
||
517 | case GPS_CKA: |
||
518 | if (c == CK_A) |
||
519 | { |
||
520 | gpsState = GPS_CKB; |
||
521 | } |
||
522 | else |
||
523 | { |
||
524 | gpsState = GPS_EMPTY; |
||
525 | } |
||
526 | break; |
||
527 | case GPS_CKB: |
||
528 | if (c == CK_B && ubxSp) // No error -> packet received successfully |
||
529 | { |
||
530 | *ubxSp = 1; // set packetStatus in struct |
||
531 | GPSscanData(); |
||
532 | } |
||
533 | gpsState = GPS_EMPTY; // ready for next packet |
||
534 | break; |
||
535 | default: |
||
536 | gpsState = GPS_EMPTY; // ready for next packet |
||
537 | } |
||
538 | } |
||
539 | else // discard any data if error occured |
||
540 | { |
||
541 | gpsState = GPS_EMPTY; |
||
542 | } |
||
543 | } |
||
544 | |||
545 | |||
546 | |||
547 |