| 375,10 → 375,10 |
|---|
| diff_east += dist_east; // Differenz zur vorhergehenden East Position |
| diff_north += dist_north; // Differenz zur vorhergehenden North Position |
| |
| diff_east_f = ((diff_east_f )/2) + (diff_east /2); //Differenzierer filtern |
| diff_north_f = ((diff_north_f)/2) + (diff_north/2); //Differenzierer filtern |
| diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern |
| diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern |
| |
| #define GPSINT_MAX 2048 //neuer Wert ab 25.9.2007 Begrenzung |
| #define GPSINT_MAX 2048 //neuer Wert ab 1.10.2007 Begrenzung |
| if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
| { |
| int_east -= dist_east; |
| 402,11 → 402,11 |
|---|
| dist_north_p = (int) d; |
| |
| //PID Regler Werte aufsummieren |
| gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east_p * Parameter_UserParam1)/4)+ ((diff_east_f * Parameter_UserParam3)/2); // I + P +D Anteil X Achse |
| gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north_p * Parameter_UserParam1)/4)+ ((diff_north_f * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
| gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east_p * Parameter_UserParam1)/8)+ ((diff_east_f * Parameter_UserParam3)); // I + P +D Anteil X Achse |
| gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north_p * Parameter_UserParam1)/8)+ ((diff_north_f * Parameter_UserParam3)); // I + P +D Anteil Y Achse |
| |
| //Ziel-Richtung bezogen auf Nordpol bestimmen |
| GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
| GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |
| |
| // in Winkel 0...360 Grad umrechnen |
| if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
| 435,7 → 435,7 |
|---|
| GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
| GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
| |
| #define GPS_V 8 |
| #define GPS_V 8 // auf Maximalwert normieren. Teilerfaktor ist 8 |
| if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
| else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
| if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |