Rev 602 | 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 |