| 64,7 → 64,10 |
|---|
| static uint8_t *ptr_payload_data; |
| static uint8_t *ptr_pac_status; |
| long int dist_flown; |
| unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
| signed int int_east,int_north; //Integrierer |
| |
| |
| short int Get_GPS_data(void); |
| |
| NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt |
| 295,7 → 298,6 |
|---|
| short int GPS_CRTL(short int cmd) |
| { |
| static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| static signed int int_east,int_north; //Integrierer |
| 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 |
| 303,6 → 305,7 |
|---|
| 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; |
| |
| switch (cmd) |
| { |
| |
| 372,6 → 375,7 |
|---|
| dist_east = 0, dist_north = 0; |
| diff_east_f = 0, diff_north_f= 0; |
| diff_east = 0, diff_north = 0; |
| int_ovfl_cnt = 0; |
| gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
| gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
| gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
| 501,7 → 505,7 |
|---|
| { |
| gps_updte_flag = 0; |
| // ab hier wird geregelt |
| diff_east = -dist_east; //Alten Wert fuer Differenzier schon mal abziehen |
| diff_east = -dist_east; //Alten Wert fuer Differenzierer schon mal abziehen |
| diff_north = -dist_north; |
| dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
| dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| 521,12 → 525,20 |
|---|
| diff_north_f = ((diff_north_f * 1)/4) + ((diff_north*3)/4); //Differenzierer filtern |
| } |
| |
| #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung |
| if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
| #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
| if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
| { |
| int_east -= dist_east; |
| int_north -= dist_north; |
| if (int_ovfl_cnt < 40) int_ovfl_cnt += 20; // Zahl der Overflows zaehlen |
| // int_east -= dist_east; auf alten Wert halten |
| // int_north -= dist_north; |
| } |
| if (int_ovfl_cnt > 0) //bei Overflow Wert Inetgratorwert reduzieren |
| { |
| int_ovfl_cnt -= 1; |
| int_east = (int_east*7)/8; // Wert reduzieren |
| int_north = (int_north*7)/8; |
| } |
| |
| if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
| { |
| int_east = 0; |
| 533,6 → 545,7 |
|---|
| int_north = 0; |
| } |
| |
| |
| // 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; |
| 556,7 → 569,7 |
|---|
| else diff_p = GPS_PROP_NRML_V; |
| |
| //I Werte begrenzen |
| #define INT1_MAX (20 * GPS_V) |
| #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V)/2 //ab 30.12.2007 auf Halben Maximalen Nick/Rollwert begrenzen |
| int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; //Fehler behoben am 17.12.2007 vorher int_north= |
| if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |