| 66,8 → 66,10 |
|---|
| long int dist_flown; |
| unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
| signed int int_east,int_north; //Integrierer |
| signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
| signed int diff_v; |
| signed long dist; |
| |
| |
| short int Get_GPS_data(void); |
| |
| NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt |
| 300,8 → 302,7 |
|---|
| static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| static signed int dist_north,dist_east; |
| static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
| static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
| signed int n,diff_v; |
| signed int n; |
| static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
| long signed int dev,n_l; |
| signed int dist_frm_start_east,dist_frm_start_north; |
| 521,8 → 522,8 |
|---|
| } |
| else // schwache Filterung |
| { |
| diff_east_f = ((diff_east_f * 1)/4) + ((diff_east *3)/4); //Differenzierer filtern |
| diff_north_f = ((diff_north_f * 1)/4) + ((diff_north*3)/4); //Differenzierer filtern |
| diff_east_f = ((diff_east_f * 2)/4) + ((diff_east *2)/4); //Differenzierer filtern |
| diff_north_f = ((diff_north_f * 2)/4) + ((diff_north*2)/4); //Differenzierer filtern |
| } |
| |
| #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
| 548,7 → 549,8 |
|---|
| |
| // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
| // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
| signed long dist,int_east1,int_north1; |
| signed long int_east1,int_north1; |
| |
| int phi; |
| phi = arctan_i(abs(dist_north),abs(dist_east)); |
| dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
| 555,15 → 557,18 |
|---|
| |
| if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer |
| { |
| diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10 |
| if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen |
| // diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10 |
| // if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen |
| diff_v = GPS_DIFF_NRML_MAX_V ; //variable Versterkung raus 31.12.2007 |
| } |
| else |
| { |
| diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10 |
| if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen |
| // diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10 |
| // if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen |
| diff_v = GPS_DIFF_FAST_MAX_V ; //variable Versterkung raus 31.12.2007 |
| } |
| |
| |
| int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
| if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
| else diff_p = GPS_PROP_NRML_V; |