283,8 → 283,10 |
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
short int GPS_CRTL(short int cmd) |
{ |
static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
static int int_east,int_north; //Integrierer |
static signed int dist_north,dist_east; |
int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
int n; |
long int dist; |
switch (cmd) |
349,18 → 351,22 |
return (GPS_STST_OK); |
break; |
|
case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen |
{ |
gps_updte_flag = 0; |
// ab hier wird geregelt |
signed int dist_north,dist_east; |
|
diff_east = -dist_east; //Alten Wert schon mal abziehen |
diff_north = -dist_north; |
dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
int_east += dist_east; |
int_north += dist_north; |
diff_east += dist_east; // Differenz zur vorhergehenden East Position |
diff_north += dist_north; // Differenz zur vorhergehenden North Position |
|
#define GPSINT_MAX 1000 |
#define GPSINT_MAX 2048 //neuer Wert ab 25.9.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; |
367,15 → 373,15 |
int_north -= dist_north; |
} |
|
#define DIST_MAX 200 //neu ab 19.9. 1900 Begrenzung |
#define DIST_MAX 100 //neuer Wert ab 24.9.2007 Begrenzung |
if (dist_east > DIST_MAX) dist_east = DIST_MAX; |
if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
|
//PI Regler |
gps_reg_x = ((int_east * Parameter_UserParam1)/16) + ((dist_east * Parameter_UserParam2)/8); // P+I Anteil |
gps_reg_y = ((int_north * Parameter_UserParam1)/16) + ((dist_north * Parameter_UserParam2)/8); // P+I Anteil |
//PID Regler |
gps_reg_x = ((int_east * Parameter_UserParam1)/32) + ((dist_east * Parameter_UserParam2)/2)+ ((diff_east * Parameter_UserParam3)/2); // |
gps_reg_y = ((int_north * Parameter_UserParam1)/32) + ((dist_north * Parameter_UserParam2)/2)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
|
//Richtung bezogen auf Nordpol bestimmen |
GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
387,7 → 393,7 |
// Relative Richtung in bezug auf Nordachse des Kopters errechen |
n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n; |
if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180))GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360; |
if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360; |
else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt; |
else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
|