| 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 |
| 345,7 → 345,7 |
|---|
| 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; |
| |
| |
| //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; |
| 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); |