72,7 → 72,6 |
// Initialisierung |
void GPS_Neutral(void) |
{ |
short int n; |
ublox_msg_state = UBLOX_IDLE; |
gps_state = GPS_CRTL_IDLE; |
actual_pos.status = 0; |
285,7 → 284,8 |
short int GPS_CRTL(short int cmd) |
{ |
static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
|
int n; |
long int dist; |
switch (cmd) |
{ |
case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden. |
292,7 → 292,7 |
if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
{ |
cnt++; |
if (cnt > 300) // erst nach Verzoegerung |
if (cnt > 500) // erst nach Verzoegerung |
{ |
cnt = 0; |
// aktuelle positionsdaten abespeichern |
361,10 → 361,9 |
GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
|
//in Winkel 0..360 grad umrechnen |
if ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
else if ((dist_east < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
if ((gps_reg_x >= 0) && (gps_reg_y < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
else if ((gps_reg_x < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
// Relative Richtung in bezug auf Nordachse des Kopters errechen |
int n,m; |
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; |
371,8 → 370,31 |
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; |
|
// Distanz zum Ziel ermitteln |
GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt |
// 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) cos_i(GPS_hdng_rel_2trgt)); |
} |
else |
{ |
dist = (long)gps_reg_y; |
dist = abs((dist *1000) / (long) sin_i(GPS_hdng_rel_2trgt)); |
} |
if (dist > 30000) dist = 30000; |
GPS_dist_2trgt = (int )dist; |
|
// Winkel und Distanz in Nick und Roll groessen umrechnen |
long int a,b; |
GPS_Roll = (int) -((dist * sin_i(GPS_hdng_rel_2trgt))/(1000*4)); |
GPS_Nick = (int) -((dist * cos_i(GPS_hdng_rel_2trgt))/(1000*4)); |
|
if (GPS_Roll > GPS_ROLL_MAX) GPS_Roll = GPS_ROLL_MAX; //Auf Maxwerte begrenzen |
else if (GPS_Roll < -GPS_ROLL_MAX) GPS_Roll = - GPS_ROLL_MAX; |
if (GPS_Nick > GPS_NICK_MAX) GPS_Nick = GPS_NICK_MAX; |
else if (GPS_Nick < -GPS_NICK_MAX) GPS_Nick = - GPS_NICK_MAX; |
|
return (GPS_STST_OK); |
} |
else return (GPS_STST_OK); |