Rev 266 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 266 | Rev 291 | ||
|---|---|---|---|
| Line 14... | Line 14... | ||
| 14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 15 | Peter Muehlenbrock |
15 | Peter Muehlenbrock |
| 16 | Auswertung der Daten vom GPS im ublox Format |
16 | Auswertung der Daten vom GPS im ublox Format |
| 17 | Hold Modus mit PID Regler |
17 | Hold Modus mit PID Regler |
| 18 | Rückstuerz zur Basis Funktion |
18 | Rückstuerz zur Basis Funktion |
| 19 | Stand 8.10.2007 |
19 | Stand 12.10.2007 |
| 20 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
20 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 21 | */ |
21 | */ |
| 22 | #include "main.h" |
22 | #include "main.h" |
| 23 | //#include "gps.h" |
23 | //#include "gps.h" |
| Line 501... | Line 501... | ||
| 501 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
501 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| 502 | int_east += dist_east; |
502 | int_east += dist_east; |
| 503 | int_north += dist_north; |
503 | int_north += dist_north; |
| 504 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
504 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
| 505 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
505 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
| 506 | /* |
506 | |
| - | 507 | if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen |
|
| - | 508 | { |
|
| - | 509 | diff_east_f = ((diff_east_f *3)/4) + (diff_east *1/4); //Differenzierer filtern |
|
| - | 510 | diff_north_f = ((diff_north_f *3)/4) + (diff_north*1/4); //Differenzierer filtern |
|
| - | 511 | } |
|
| - | 512 | else // schwache Filterung |
|
| - | 513 | { |
|
| - | 514 | // diff_east_f = diff_east; |
|
| - | 515 | // diff_north_f = diff_north; |
|
| 507 | diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern |
516 | diff_east_f = ((diff_east_f *2)/4) + (diff_east *2/4); //Differenzierer filtern |
| 508 | diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern |
517 | diff_north_f = ((diff_north_f *2)/4) + (diff_north*2/4); //Differenzierer filtern |
| - | 518 | } |
|
| 509 | */ |
519 | |
| 510 | #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung |
520 | #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung |
| 511 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
521 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
| 512 | { |
522 | { |
| 513 | int_east -= dist_east; |
523 | int_east -= dist_east; |
| 514 | int_north -= dist_north; |
524 | int_north -= dist_north; |
| Line 549... | Line 559... | ||
| 549 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
559 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
| 550 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
560 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
| 551 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
561 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
| Line 552... | Line 562... | ||
| 552 | 562 | ||
| 553 | //PID Regler Werte aufsummieren |
563 | //PID Regler Werte aufsummieren |
| 554 | gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
564 | gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east_f * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
| Line 555... | Line 565... | ||
| 555 | gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_north * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |
565 | gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_north_f * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |
| 556 | 566 | ||
| Line 557... | Line 567... | ||
| 557 | //Ziel-Richtung bezogen auf Nordpol bestimmen |
567 | //Ziel-Richtung bezogen auf Nordpol bestimmen |