Rev 565 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 565 | Rev 602 | ||
|---|---|---|---|
| Line 62... | Line 62... | ||
| 62 | static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
62 | static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
| 63 | static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
63 | static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
| 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 |
|
| - | 68 | signed int int_east,int_north; //Integrierer |
|
| - | 69 | ||
| Line 67... | Line 70... | ||
| 67 | 70 | ||
| Line 68... | Line 71... | ||
| 68 | short int Get_GPS_data(void); |
71 | short int Get_GPS_data(void); |
| 69 | 72 | ||
| Line 293... | Line 296... | ||
| 293 | 296 | ||
| 294 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
297 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
| 295 | short int GPS_CRTL(short int cmd) |
298 | short int GPS_CRTL(short int cmd) |
| 296 | { |
299 | { |
| 297 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
- | |
| 298 | static signed int int_east,int_north; //Integrierer |
300 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| 299 | static signed int dist_north,dist_east; |
301 | static signed int dist_north,dist_east; |
| 300 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
302 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
| 301 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
303 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
| 302 | signed int n,diff_v; |
304 | signed int n,diff_v; |
| 303 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
305 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
| 304 | long signed int dev,n_l; |
306 | long signed int dev,n_l; |
| - | 307 | signed int dist_frm_start_east,dist_frm_start_north; |
|
| 305 | signed int dist_frm_start_east,dist_frm_start_north; |
308 | |
| 306 | switch (cmd) |
309 | switch (cmd) |
| Line 307... | Line 310... | ||
| 307 | { |
310 | { |
| 308 | 311 | ||
| Line 370... | Line 373... | ||
| 370 | int_east = 0, int_north = 0; |
373 | int_east = 0, int_north = 0; |
| 371 | gps_reg_x = 0, gps_reg_y = 0; |
374 | gps_reg_x = 0, gps_reg_y = 0; |
| 372 | dist_east = 0, dist_north = 0; |
375 | dist_east = 0, dist_north = 0; |
| 373 | diff_east_f = 0, diff_north_f= 0; |
376 | diff_east_f = 0, diff_north_f= 0; |
| 374 | diff_east = 0, diff_north = 0; |
377 | diff_east = 0, diff_north = 0; |
| - | 378 | int_ovfl_cnt = 0; |
|
| 375 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
379 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
| 376 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
380 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
| 377 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
381 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
| 378 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
382 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
| 379 | return (GPS_STST_OK); |
383 | return (GPS_STST_OK); |
| Line 499... | Line 503... | ||
| 499 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
503 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
| 500 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
504 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
| 501 | { |
505 | { |
| 502 | gps_updte_flag = 0; |
506 | gps_updte_flag = 0; |
| 503 | // ab hier wird geregelt |
507 | // ab hier wird geregelt |
| 504 | diff_east = -dist_east; //Alten Wert fuer Differenzier schon mal abziehen |
508 | diff_east = -dist_east; //Alten Wert fuer Differenzierer schon mal abziehen |
| 505 | diff_north = -dist_north; |
509 | diff_north = -dist_north; |
| 506 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
510 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
| 507 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
511 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| 508 | int_east += dist_east; |
512 | int_east += dist_east; |
| 509 | int_north += dist_north; |
513 | int_north += dist_north; |
| Line 519... | Line 523... | ||
| 519 | { |
523 | { |
| 520 | diff_east_f = ((diff_east_f * 1)/4) + ((diff_east *3)/4); //Differenzierer filtern |
524 | diff_east_f = ((diff_east_f * 1)/4) + ((diff_east *3)/4); //Differenzierer filtern |
| 521 | diff_north_f = ((diff_north_f * 1)/4) + ((diff_north*3)/4); //Differenzierer filtern |
525 | diff_north_f = ((diff_north_f * 1)/4) + ((diff_north*3)/4); //Differenzierer filtern |
| 522 | } |
526 | } |
| Line 523... | Line 527... | ||
| 523 | 527 | ||
| 524 | #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung |
528 | #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 | { |
|
| - | 531 | if (int_ovfl_cnt < 40) int_ovfl_cnt += 20; // Zahl der Overflows zaehlen |
|
| - | 532 | // int_east -= dist_east; auf alten Wert halten |
|
| - | 533 | // int_north -= dist_north; |
|
| - | 534 | } |
|
| 525 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
535 | if (int_ovfl_cnt > 0) //bei Overflow Wert Inetgratorwert reduzieren |
| 526 | { |
536 | { |
| - | 537 | int_ovfl_cnt -= 1; |
|
| 527 | int_east -= dist_east; |
538 | int_east = (int_east*7)/8; // Wert reduzieren |
| 528 | int_north -= dist_north; |
539 | int_north = (int_north*7)/8; |
| - | 540 | } |
|
| 529 | } |
541 | |
| 530 | if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
542 | if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
| 531 | { |
543 | { |
| 532 | int_east = 0; |
544 | int_east = 0; |
| 533 | int_north = 0; |
545 | int_north = 0; |
| Line -... | Line 546... | ||
| - | 546 | } |
|
| 534 | } |
547 | |
| 535 | 548 | ||
| 536 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
549 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
| 537 | // 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 |
| 538 | signed long dist,int_east1,int_north1; |
551 | signed long dist,int_east1,int_north1; |
| Line 554... | Line 567... | ||
| 554 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
567 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
| 555 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
568 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
| 556 | else diff_p = GPS_PROP_NRML_V; |
569 | else diff_p = GPS_PROP_NRML_V; |
| Line 557... | Line 570... | ||
| 557 | 570 | ||
| 558 | //I Werte begrenzen |
571 | //I Werte begrenzen |
| 559 | #define INT1_MAX (20 * GPS_V) |
572 | #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V)/2 //ab 30.12.2007 auf Halben Maximalen Nick/Rollwert begrenzen |
| 560 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
573 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| 561 | int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; //Fehler behoben am 17.12.2007 vorher int_north= |
574 | int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; //Fehler behoben am 17.12.2007 vorher int_north= |
| 562 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
575 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
| 563 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
576 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |