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