Rev 190 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 190 | Rev 194 | ||
|---|---|---|---|
| Line 373... | Line 373... | ||
| 373 | int_east += dist_east; |
373 | int_east += dist_east; |
| 374 | int_north += dist_north; |
374 | int_north += dist_north; |
| 375 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
375 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
| 376 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
376 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
| Line 377... | Line 377... | ||
| 377 | 377 | ||
| 378 | diff_east_f = ((diff_east_f )/2) + (diff_east /2); //Differenzierer filtern |
378 | diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern |
| Line 379... | Line 379... | ||
| 379 | diff_north_f = ((diff_north_f)/2) + (diff_north/2); //Differenzierer filtern |
379 | diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern |
| 380 | 380 | ||
| 381 | #define GPSINT_MAX 2048 //neuer Wert ab 25.9.2007 Begrenzung |
381 | #define GPSINT_MAX 2048 //neuer Wert ab 1.10.2007 Begrenzung |
| 382 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
382 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
| 383 | { |
383 | { |
| 384 | int_east -= dist_east; |
384 | int_east -= dist_east; |
| Line 400... | Line 400... | ||
| 400 | n = sin_i((dist_north_p*90)/(GPS_DIST_P_MAX)); |
400 | n = sin_i((dist_north_p*90)/(GPS_DIST_P_MAX)); |
| 401 | d = (GPS_DIST_P_MAX * (long)n) /1000; |
401 | d = (GPS_DIST_P_MAX * (long)n) /1000; |
| 402 | dist_north_p = (int) d; |
402 | dist_north_p = (int) d; |
| Line 403... | Line 403... | ||
| 403 | 403 | ||
| 404 | //PID Regler Werte aufsummieren |
404 | //PID Regler Werte aufsummieren |
| 405 | 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 |
405 | 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 |
| Line 406... | Line 406... | ||
| 406 | 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 |
406 | 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 |
| 407 | 407 | ||
| Line 408... | Line 408... | ||
| 408 | //Ziel-Richtung bezogen auf Nordpol bestimmen |
408 | //Ziel-Richtung bezogen auf Nordpol bestimmen |
| 409 | GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
409 | GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |
| 410 | 410 | ||
| Line 433... | Line 433... | ||
| 433 | GPS_dist_2trgt = (int) dist; |
433 | GPS_dist_2trgt = (int) dist; |
| 434 | // Winkel und Distanz in Nick und Rollgroessen umrechnen |
434 | // Winkel und Distanz in Nick und Rollgroessen umrechnen |
| 435 | GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
435 | GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
| 436 | GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
436 | GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
| Line 437... | Line 437... | ||
| 437 | 437 | ||
| 438 | #define GPS_V 8 |
438 | #define GPS_V 8 // auf Maximalwert normieren. Teilerfaktor ist 8 |
| 439 | if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
439 | if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
| 440 | else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
440 | else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
| 441 | if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |
441 | if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |