49,7 → 49,8 |
signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol |
signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
|
signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
signed int ; |
static unsigned int rx_len; |
unsigned int cnt0,cnt1,cnt2; //******Provisorisch |
static unsigned int ptr_payload_data_end; |
83,6 → 84,9 |
GPS_Nick = 0; |
GPS_Roll = 0; |
gps_updte_flag = 0; |
gps_int_x = 0; |
gps_int_y = 0; |
|
} |
|
// Home Position sichern falls Daten verfuegbar sind. |
316,6 → 320,8 |
GPS_Nick = 0; |
GPS_Roll = 0; |
gps_state = GPS_CRTL_IDLE; |
gps_int_x = 0; |
gps_int_y = 0; |
return (GPS_STST_OK); |
break; |
|
332,19 → 338,39 |
break; |
|
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 |
if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen |
{ |
gps_updte_flag = 0; |
// ab hier wird geregelt |
// Richtung zum Ziel ermitteln |
signed int dist_north,dist_east; |
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; |
GPS_hdng_abs_2trgt = arctan_i((long)dist_east,(long)dist_north); |
|
//PI Regler |
gps_int_x = gps_int_x + (dist_east * Parameter_UserParam1)/16; // Integrator |
gps_int_y = gps_int_y + (dist_north * Parameter_UserParam1)/16; |
if ((gps_int_x >= 4096) || (gps_int_y >= 4096) || (gps_int_x < - 4096) || (gps_int_y <= -4096)) |
{ |
gps_int_x -= (dist_east * Parameter_UserParam1)/16; // Integrator stoppen |
gps_int_y -= (dist_north * Parameter_UserParam1)/16; |
} |
gps_reg_x = gps_int_x + (dist_east * Parameter_UserParam2)/16; // P Anteil dazu |
gps_reg_y = gps_int_y + (dist_north * Parameter_UserParam2)/16; |
|
//Richtung bezogen auf Nordpol bestimmen |
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); |
|
// 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; |
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 |
return (GPS_STST_OK); |