Rev 225 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 225 | Rev 236 | ||
---|---|---|---|
Line 11... | Line 11... | ||
11 | */ |
11 | */ |
12 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
13 | Peter Muehlenbrock |
13 | Peter Muehlenbrock |
14 | Auswertung der Daten vom GPS im ublox Format |
14 | Auswertung der Daten vom GPS im ublox Format |
15 | Hold Modus mit PID Regler |
15 | Hold Modus mit PID Regler |
16 | Komm heim zu Papi Funktion |
16 | Rückstuerz zur Basis Funktion |
17 | Stand 6.10.2007 |
17 | Stand 6.10.2007 |
18 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
18 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
19 | */ |
19 | */ |
20 | #include "main.h" |
20 | #include "main.h" |
21 | //#include "gps.h" |
21 | //#include "gps.h" |
Line 54... | Line 54... | ||
54 | static unsigned int rx_len; |
54 | static unsigned int rx_len; |
55 | static unsigned int ptr_payload_data_end; |
55 | static unsigned int ptr_payload_data_end; |
56 | unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
56 | unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
57 | signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
57 | signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
58 | signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
58 | signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
59 | static short int hold_fast; //Flag fuer Hold Regler |
59 | static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
60 | static uint8_t *ptr_payload_data; |
60 | static uint8_t *ptr_payload_data; |
61 | static uint8_t *ptr_pac_status; |
61 | static uint8_t *ptr_pac_status; |
62 | long int dist_flown; |
62 | long int dist_flown; |
Line 63... | Line 63... | ||
63 | 63 | ||
Line 284... | Line 284... | ||
284 | } |
284 | } |
Line 285... | Line 285... | ||
285 | 285 | ||
286 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
286 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
287 | short int GPS_CRTL(short int cmd) |
287 | short int GPS_CRTL(short int cmd) |
288 | { |
288 | { |
289 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
289 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
290 | static signed int int_east,int_north; //Integrierer |
290 | static signed int int_east,int_north; //Integrierer |
291 | static signed int dist_north,dist_east; |
291 | static signed int dist_north,dist_east; |
292 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
292 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
293 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
293 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
- | 294 | signed int n,diff_v; |
|
294 | signed int n,diff_v; |
295 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
295 | long signed int dev,n_l; |
296 | long signed int dev,n_l; |
296 | signed int dist_frm_start_east,dist_frm_start_north; |
297 | signed int dist_frm_start_east,dist_frm_start_north; |
297 | switch (cmd) |
298 | switch (cmd) |
Line 305... | Line 306... | ||
305 | { |
306 | { |
306 | // Erst mal initialisieren |
307 | // Erst mal initialisieren |
307 | cnt = 0; |
308 | cnt = 0; |
308 | gps_tick = 0; |
309 | gps_tick = 0; |
309 | hold_fast = 0; |
310 | hold_fast = 0; |
- | 311 | hold_reset_int = 0; // Integrator enablen |
|
310 | int_east = 0, int_north = 0; |
312 | int_east = 0, int_north = 0; |
311 | gps_reg_x = 0, gps_reg_y = 0; |
313 | gps_reg_x = 0, gps_reg_y = 0; |
312 | dist_east = 0, dist_north = 0; |
314 | dist_east = 0, dist_north = 0; |
313 | diff_east_f = 0, diff_north_f= 0; |
315 | diff_east_f = 0, diff_north_f= 0; |
314 | diff_east = 0, diff_north = 0; |
316 | diff_east = 0, diff_north = 0; |
315 | dist_flown = 0; |
317 | dist_flown = 0; |
- | 318 | gps_g2t_act_v = 0; |
|
316 | gps_sub_state = GPS_CRTL_IDLE; |
319 | gps_sub_state = GPS_CRTL_IDLE; |
317 | // aktuelle positionsdaten abspeichern |
320 | // aktuelle positionsdaten abspeichern |
318 | if (gps_rel_act_position.status > 0) |
321 | if (gps_rel_act_position.status > 0) |
319 | { |
322 | { |
320 | gps_rel_start_position.utm_east = gps_rel_act_position.utm_east; |
323 | gps_rel_start_position.utm_east = gps_rel_act_position.utm_east; |
Line 352... | Line 355... | ||
352 | { |
355 | { |
353 | cnt = 0; |
356 | cnt = 0; |
354 | // aktuelle positionsdaten abspeichern |
357 | // aktuelle positionsdaten abspeichern |
355 | if (gps_rel_act_position.status > 0) |
358 | if (gps_rel_act_position.status > 0) |
356 | { |
359 | { |
357 | hold_fast = 0; |
360 | hold_fast = 0; |
- | 361 | hold_reset_int = 0; // Integrator enablen |
|
358 | int_east = 0, int_north = 0; |
362 | int_east = 0, int_north = 0; |
359 | gps_reg_x = 0, gps_reg_y = 0; |
363 | gps_reg_x = 0, gps_reg_y = 0; |
360 | dist_east = 0, dist_north = 0; |
364 | dist_east = 0, dist_north = 0; |
361 | diff_east_f = 0, diff_north_f= 0; |
365 | diff_east_f = 0, diff_north_f= 0; |
362 | diff_east = 0, diff_north = 0; |
366 | diff_east = 0, diff_north = 0; |
Line 414... | Line 418... | ||
414 | if (d3 > GPS_G2T_DIST_MAX_STOP) |
418 | if (d3 > GPS_G2T_DIST_MAX_STOP) |
415 | { |
419 | { |
416 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
420 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
417 | { |
421 | { |
418 | hold_fast = 1; // Schnell Regeln |
422 | hold_fast = 1; // Schnell Regeln |
- | 423 | if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
|
419 | dist_flown += (GPS_G2T_V_MAX); // Vorgabe der Strecke anhand der Geschwindigkeit |
424 | dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
420 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
425 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
421 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
426 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
422 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
427 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
423 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
428 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
424 | } |
429 | } |
- | 430 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
|
- | 431 | { |
|
- | 432 | if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
|
425 | else hold_fast = 0; // Wieder normal regeln |
433 | if (gps_g2t_act_v <= (GPS_G2T_V_MAX/2)) hold_fast = 0; // Wieder normal regeln |
- | 434 | } |
|
426 | } |
435 | } |
427 | else //Schon ziemlich nahe am Ziel, deswegen abbremsen |
436 | else //Schon ziemlich nahe am Ziel, deswegen abbremsen |
428 | { |
437 | { |
429 | hold_fast = 0; // Wieder normal regeln |
438 | hold_fast = 0; // Wieder normal regeln |
430 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
439 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
431 | { |
440 | { |
432 | if (d3 > GPS_G2T_DIST_HOLD) |
441 | if (d3 > GPS_G2T_DIST_HOLD) |
433 | { |
442 | { |
- | 443 | if (gps_g2t_act_v < GPS_G2T_V_RAMP_DWN) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
|
434 | dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit |
444 | dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
435 | dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000; |
445 | dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000; |
436 | dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000; |
446 | dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000; |
437 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
447 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
438 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt |
448 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt |
439 | } |
449 | } |
Line 443... | Line 453... | ||
443 | else if (gps_rel_hold_position.utm_east <-1 ) gps_rel_hold_position.utm_east++; |
453 | else if (gps_rel_hold_position.utm_east <-1 ) gps_rel_hold_position.utm_east++; |
444 | if (gps_rel_hold_position.utm_north > 1) gps_rel_hold_position.utm_north--; |
454 | if (gps_rel_hold_position.utm_north > 1) gps_rel_hold_position.utm_north--; |
445 | else if (gps_rel_hold_position.utm_north < -1 ) gps_rel_hold_position.utm_north++; |
455 | else if (gps_rel_hold_position.utm_north < -1 ) gps_rel_hold_position.utm_north++; |
446 | if ((abs(gps_rel_hold_position.utm_east) <= 1) && (abs(gps_rel_hold_position.utm_north) <=1)) gps_sub_state = GPS_HOME_FINISHED; |
456 | if ((abs(gps_rel_hold_position.utm_east) <= 1) && (abs(gps_rel_hold_position.utm_north) <=1)) gps_sub_state = GPS_HOME_FINISHED; |
447 | } |
457 | } |
448 | } |
458 | } |
- | 459 | else |
|
- | 460 | { |
|
- | 461 | if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
|
- | 462 | } |
|
449 | } |
463 | } |
450 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
464 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
451 | return (GPS_STST_OK); |
465 | return (GPS_STST_OK); |
452 | } |
466 | } |
453 | else return (GPS_STST_OK); |
467 | else return (GPS_STST_OK); |
Line 458... | Line 472... | ||
458 | return (GPS_STST_ERR); |
472 | return (GPS_STST_ERR); |
459 | } |
473 | } |
460 | break; |
474 | break; |
Line 461... | Line 475... | ||
461 | 475 | ||
462 | 476 | ||
463 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
477 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
464 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
478 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
465 | { |
479 | { |
466 | gps_updte_flag = 0; |
480 | gps_updte_flag = 0; |
Line 475... | Line 489... | ||
475 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
489 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
476 | /* |
490 | /* |
477 | diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern |
491 | diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern |
478 | diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern |
492 | diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern |
479 | */ |
493 | */ |
480 | #define GPSINT_MAX 2048 //neuer Wert ab 1.10.2007 Begrenzung |
494 | #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung |
481 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
495 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
482 | { |
496 | { |
483 | int_east -= dist_east; |
497 | int_east -= dist_east; |
484 | int_north -= dist_north; |
498 | int_north -= dist_north; |
485 | } |
499 | } |
486 | if (hold_fast > 0) //Im Schnellen Mode Integrator abschalten |
500 | if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
487 | { |
501 | { |
488 | int_east = 0; |
502 | int_east = 0; |
489 | int_north = 0; |
503 | int_north = 0; |
490 | } |
504 | } |
Line 491... | Line 505... | ||
491 | 505 | ||
492 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
506 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
493 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
507 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
494 | #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10 |
508 | #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10 |
495 | #define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm |
509 | #define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm |
496 | signed long dist; |
510 | signed long dist,int_east1,int_north1; |
497 | int phi; |
511 | int phi; |
498 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
512 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
Line 499... | Line 513... | ||
499 | dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
513 | dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
500 | 514 | ||
501 | diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10 |
515 | diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10 |
- | 516 | if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen |
|
- | 517 | if (hold_fast > 0) diff_v = 10; //im schnellen Modus schwache Wirkung des Differenzierers |
|
- | 518 | ||
- | 519 | //I Werte begrenzen |
|
- | 520 | #define INT1_MAX (20 * GPS_V) |
|
- | 521 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32); |
|
- | 522 | int_north = ((((long)int_north) * Parameter_UserParam2)/32); |
|
- | 523 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
|
- | 524 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
|
Line 502... | Line 525... | ||
502 | if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen |
525 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
503 | if (hold_fast > 0) diff_v = 10; // im schnellen Modus keine staerkere Wirkung des Differenzierers |
526 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
504 | 527 | ||
Line 505... | Line 528... | ||
505 | //PID Regler Werte aufsummieren |
528 | //PID Regler Werte aufsummieren |
506 | gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse |
529 | gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
Line 507... | Line 530... | ||
507 | gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil Y Achse |
530 | gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |
Line 534... | Line 557... | ||
534 | GPS_dist_2trgt = (int) dev; |
557 | GPS_dist_2trgt = (int) dev; |
535 | // Winkel und Distanz in Nick und Rollgroessen umrechnen |
558 | // Winkel und Distanz in Nick und Rollgroessen umrechnen |
536 | GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
559 | GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
537 | GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
560 | GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
Line 538... | Line -... | ||
538 | - | ||
539 | #define GPS_V 8 // auf Maximalwert normieren. Teilerfaktor ist 8 |
561 | |
540 | if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
562 | if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
541 | else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
563 | else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
542 | if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |
564 | if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |