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); |