Rev 529 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 529 | Rev 542 | ||
|---|---|---|---|
| Line 552... | Line 552... | ||
| 552 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
552 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
| 553 | else diff_p = GPS_PROP_NRML_V; |
553 | else diff_p = GPS_PROP_NRML_V; |
| Line 554... | Line 554... | ||
| 554 | 554 | ||
| 555 | //I Werte begrenzen |
555 | //I Werte begrenzen |
| 556 | #define INT1_MAX (20 * GPS_V) |
556 | #define INT1_MAX (20 * GPS_V) |
| 557 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32); |
557 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| 558 | int_north = ((((long)int_north) * Parameter_UserParam2)/32); |
558 | int_north = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| 559 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
559 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
| 560 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
560 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
| 561 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
561 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
| Line 562... | Line 562... | ||
| 562 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
562 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
| 563 | 563 | ||
| 564 | //PID Regler Werte aufsummieren |
564 | //PID Regler Werte aufsummieren |
| Line 565... | Line 565... | ||
| 565 | 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 |
565 | gps_reg_x = ((int)int_east1 + ((dist_east * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((diff_east_f * diff_v * (Parameter_UserParam3/GPS_USR_PAR_FKT))/10)); // I + P +D Anteil X Achse |
| 566 | 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 |
566 | gps_reg_y = ((int)int_north1 + ((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((diff_north_f * diff_v * (Parameter_UserParam3/GPS_USR_PAR_FKT))/10)); // I + P +D Anteil Y Achse |
| Line 567... | Line 567... | ||
| 567 | 567 | ||