Rev 602 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 602 | Rev 613 | ||
|---|---|---|---|
| Line 64... | Line 64... | ||
| 64 | static uint8_t *ptr_payload_data; |
64 | static uint8_t *ptr_payload_data; |
| 65 | static uint8_t *ptr_pac_status; |
65 | static uint8_t *ptr_pac_status; |
| 66 | long int dist_flown; |
66 | long int dist_flown; |
| 67 | unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
67 | unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
| 68 | signed int int_east,int_north; //Integrierer |
68 | signed int int_east,int_north; //Integrierer |
| 69 | - | ||
| - | 69 | signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
|
| - | 70 | signed int diff_v; |
|
| - | 71 | signed long dist; |
|
| Line 70... | Line 72... | ||
| 70 | 72 | ||
| Line 71... | Line 73... | ||
| 71 | short int Get_GPS_data(void); |
73 | short int Get_GPS_data(void); |
| 72 | 74 | ||
| Line 298... | Line 300... | ||
| 298 | short int GPS_CRTL(short int cmd) |
300 | short int GPS_CRTL(short int cmd) |
| 299 | { |
301 | { |
| 300 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
302 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| 301 | static signed int dist_north,dist_east; |
303 | static signed int dist_north,dist_east; |
| 302 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
304 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
| 303 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
- | |
| 304 | signed int n,diff_v; |
305 | signed int n; |
| 305 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
306 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
| 306 | long signed int dev,n_l; |
307 | long signed int dev,n_l; |
| 307 | signed int dist_frm_start_east,dist_frm_start_north; |
308 | signed int dist_frm_start_east,dist_frm_start_north; |
| Line 308... | Line 309... | ||
| 308 | 309 | ||
| Line 519... | Line 520... | ||
| 519 | diff_east_f = ((diff_east_f *2)/3) + (diff_east *1/3); //Differenzierer filtern |
520 | diff_east_f = ((diff_east_f *2)/3) + (diff_east *1/3); //Differenzierer filtern |
| 520 | diff_north_f = ((diff_north_f *2)/3) + (diff_north*1/3); //Differenzierer filtern |
521 | diff_north_f = ((diff_north_f *2)/3) + (diff_north*1/3); //Differenzierer filtern |
| 521 | } |
522 | } |
| 522 | else // schwache Filterung |
523 | else // schwache Filterung |
| 523 | { |
524 | { |
| 524 | diff_east_f = ((diff_east_f * 1)/4) + ((diff_east *3)/4); //Differenzierer filtern |
525 | diff_east_f = ((diff_east_f * 2)/4) + ((diff_east *2)/4); //Differenzierer filtern |
| 525 | diff_north_f = ((diff_north_f * 1)/4) + ((diff_north*3)/4); //Differenzierer filtern |
526 | diff_north_f = ((diff_north_f * 2)/4) + ((diff_north*2)/4); //Differenzierer filtern |
| 526 | } |
527 | } |
| Line 527... | Line 528... | ||
| 527 | 528 | ||
| 528 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
529 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
| 529 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
530 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
| Line 546... | Line 547... | ||
| 546 | } |
547 | } |
| Line 547... | Line 548... | ||
| 547 | 548 | ||
| 548 | 549 | ||
| 549 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
550 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
| - | 551 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
|
| 550 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
552 | signed long int_east1,int_north1; |
| 551 | signed long dist,int_east1,int_north1; |
553 | |
| 552 | int phi; |
554 | int phi; |
| Line 553... | Line 555... | ||
| 553 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
555 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
| 554 | dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
556 | dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
| 555 | 557 | ||
| 556 | if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer |
558 | if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer |
| - | 559 | { |
|
| 557 | { |
560 | // diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10 |
| 558 | diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10 |
561 | // if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen |
| 559 | if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen |
562 | diff_v = GPS_DIFF_NRML_MAX_V ; //variable Versterkung raus 31.12.2007 |
| 560 | } |
563 | } |
| 561 | else |
564 | else |
| - | 565 | { |
|
| 562 | { |
566 | // diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10 |
| Line -... | Line 567... | ||
| - | 567 | // if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen |
|
| 563 | diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10 |
568 | diff_v = GPS_DIFF_FAST_MAX_V ; //variable Versterkung raus 31.12.2007 |
| 564 | if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen |
569 | } |
| 565 | } |
570 | |
| Line 566... | Line 571... | ||
| 566 | 571 | ||