| 13,7 → 13,7 |
|---|
| Peter Muehlenbrock |
| Auswertung der Daten vom GPS im ublox Format |
| Hold Modus mit PID Regler |
| Stand 29.9.2007 |
| Stand 2.10.2007 |
| ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| */ |
| #include "main.h" |
| 291,8 → 291,8 |
|---|
| static signed int dist_north,dist_east; |
| static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
| static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
| signed int n; |
| long signed int dist,d,n_l; |
| signed int n,diff_v; |
| long signed int dev,n_l; |
| switch (cmd) |
| { |
| |
| 384,26 → 384,29 |
|---|
| int_east -= dist_east; |
| int_north -= dist_north; |
| } |
| // P-Anteil Kleine Werte verstaerken, grosse abschwaechen |
| #define GPS_DIST_P_MAX 200 //maximal 20m |
| int dist_east_p,dist_north_p; |
| dist_east_p = dist_east; |
| dist_north_p = dist_north; |
| if (dist_east > GPS_DIST_P_MAX) dist_east_p = GPS_DIST_P_MAX; // P-Anteil begrenzen |
| else if (dist_east < - GPS_DIST_P_MAX) dist_east_p = -GPS_DIST_P_MAX; |
| if (dist_north > GPS_DIST_P_MAX) dist_north_p = GPS_DIST_P_MAX; // P-Anteil begrenzen |
| else if (dist_north < - GPS_DIST_P_MAX) dist_north_p = -GPS_DIST_P_MAX; |
| |
| n = sin_i((dist_east_p*90)/(GPS_DIST_P_MAX)); |
| d = (GPS_DIST_P_MAX * (long)n) /1000; |
| dist_east_p = (int) d; |
| n = sin_i((dist_north_p*90)/(GPS_DIST_P_MAX)); |
| d = (GPS_DIST_P_MAX * (long)n) /1000; |
| dist_north_p = (int) d; |
| //Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
| // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. bei 0 ist die Verstaerkung 1 |
| #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10 |
| #define GPS_DIFF_MAX_D 40 //Entfernung bei der maximale Verstaerkung erreicht wird in (dm) |
| signed long dist; |
| int phi; |
| phi = arctan_i(abs(dist_north),abs(dist_east)); |
| if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln |
| { |
| dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen |
| dist = abs((dist *1000) / (long) sin_i(phi)); |
| } |
| else |
| { |
| dist = (long)dist_north; |
| dist = abs((dist *1000) / (long) cos_i(phi)); |
| } |
| diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10 |
| if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; |
| |
| //PID Regler Werte aufsummieren |
| 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 |
| gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east_f * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse |
| gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north_f * diff_v * Parameter_UserParam3)/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); |
| 422,18 → 425,18 |
|---|
| // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
| if (abs(gps_reg_x) > abs(gps_reg_y) ) |
| { |
| dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
| dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
| dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
| dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
| } |
| else |
| { |
| dist = (long)gps_reg_y; |
| dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
| dev = (long)gps_reg_y; |
| dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
| } |
| GPS_dist_2trgt = (int) dist; |
| GPS_dist_2trgt = (int) dev; |
| // Winkel und Distanz in Nick und Rollgroessen umrechnen |
| GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
| GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
| GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
| GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
| |
| #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); |