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