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; |