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