| 554,8 → 554,8 |
|---|
| |
| //I Werte begrenzen |
| #define INT1_MAX (20 * GPS_V) |
| int_east1 = ((((long)int_east) * Parameter_UserParam2)/32); |
| int_north = ((((long)int_north) * Parameter_UserParam2)/32); |
| int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| int_north = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
| else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
| if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
| 562,8 → 562,8 |
|---|
| else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
| |
| //PID Regler Werte aufsummieren |
| 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 |
| 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 |
| 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 |
| 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 |
| |
| //Ziel-Richtung bezogen auf Nordpol bestimmen |
| GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |