| 16,7 → 16,7 |
|---|
| Auswertung der Daten vom GPS im ublox Format |
| Hold Modus mit PID Regler |
| Rückstuerz zur Basis Funktion |
| Stand 9.1.2008 |
| Stand 10.1.2008 |
| |
| ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| */ |
| 54,7 → 54,8 |
|---|
| static signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol |
| static signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
| static signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
| static signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
| static signed int gps_int_x,gps_int_y; |
| static long signed gps_reg_x,gps_reg_y; |
| static unsigned int rx_len; |
| static unsigned int ptr_payload_data_end; |
| unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
| 304,7 → 305,7 |
|---|
| signed int dist_frm_start_east,dist_frm_start_north; |
| int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil |
| static signed int int_east,int_north; //Integrierer |
| int speed_east,speed_north; //Aktuelle Geschwindigkeit |
| long int speed_east,speed_north; //Aktuelle Geschwindigkeit |
| signed long int_east1,int_north1; |
| // signed long dist; |
| |
| 426,9 → 427,9 |
|---|
| d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
| d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
| d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel |
| debug_gp_2 = dist_2home; // zum Debuggen |
| debug_gp_3 = dist_flown; // zum Debuggen |
| debug_gp_4 = hdng_2home; // zum Debuggen |
| // debug_gp_2 = dist_2home; // zum Debuggen |
| // debug_gp_3 = dist_flown; // zum Debuggen |
| // debug_gp_4 = hdng_2home; // zum Debuggen |
| // debug_gp_5 = amplfy_speed_north; // zum Debuggen |
| |
| if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
| 507,13 → 508,16 |
|---|
| if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
| { |
| // ab hier wird geregelt |
| 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; |
| dist_east = gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east; |
| dist_north = gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north; |
| int_east += dist_east; |
| int_north += dist_north; |
| speed_east = (int) (-actual_speed.speed_e); |
| speed_north = (int) (-actual_speed.speed_n); |
| speed_east = actual_speed.speed_e; |
| speed_north = actual_speed.speed_n; |
| gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
| debug_gp_2 = dist_east; // zum Debuggen |
| debug_gp_3 = dist_north; // zum Debuggen |
| |
| |
| #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
| if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
| 541,8 → 545,6 |
|---|
| if (amplfy_speed_north > (DIFF_Y_F_MAX *10)) amplfy_speed_north = DIFF_Y_F_MAX *10; // Begrenzung |
| amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| amplfy_speed_east /= 10; //Faktor 10 wieder rausnehmen |
| amplfy_speed_north /= 10; //Faktor 10 wieder rausnehmen |
| } |
| else //langsamer Holdmodus |
| { |
| 553,15 → 555,14 |
|---|
| if (amplfy_speed_north > (DIFF_Y_N_MAX *10)) amplfy_speed_north = DIFF_Y_N_MAX *10; // Begrenzung |
| amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| amplfy_speed_east /= 10; //Faktor 10 wieder rausnehmen |
| amplfy_speed_north /= 10; //Faktor 10 wieder rausnehmen |
| speed_east = speed_east * abs(speed_east) /100; //quadrieren |
| speed_north = speed_north * abs(speed_north) /100; //quadrieren |
| } |
| // amplfy_speed_east /= 10; |
| // amplfy_speed_north /= 10; |
| debug_gp_4 = (int)speed_east; // zum Debuggen |
| debug_gp_5 = (int)speed_north; // zum Debuggen |
| |
| |
| #define GPS_SPEED_SCALE 5 |
| speed_east /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
| speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
| |
| int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
| if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
| else diff_p = GPS_PROP_NRML_V; |
| 576,13 → 577,13 |
|---|
| else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
| |
| //PID Regler Werte aufsummieren |
| gps_reg_x = ((int)int_east1 + ((dist_east * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_east * amplfy_speed_east )/(10/GPS_SPEED_SCALE))); // I + P +D Anteil X Achse |
| gps_reg_y = ((int)int_north1 + ((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_north * amplfy_speed_north )/(10/GPS_SPEED_SCALE))); // I + P +D Anteil Y Achse |
| debug_gp_0 = gps_reg_x; // zum Debuggen |
| debug_gp_1 = gps_reg_y; // zum Debuggen |
| gps_reg_x = -(int_east1 + (long)((dist_east * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_east * (long)amplfy_speed_east )/(100L))); // I + P +D Anteil X Achse |
| gps_reg_y = -(int_north1 + (long)((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_north * (long)amplfy_speed_north )/(100L))); // I + P +D Anteil Y Achse |
| debug_gp_0 = (int)gps_reg_x; // zum Debuggen |
| debug_gp_1 = (int)gps_reg_y; // zum Debuggen |
| |
| //Ziel-Richtung bezogen auf Nordpol bestimmen |
| GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |
| GPS_hdng_abs_2trgt = arctan_i((int)gps_reg_x,(int)gps_reg_y); |
| |
| // in Winkel 0...360 Grad umrechnen |
| if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
| 598,7 → 599,7 |
|---|
| // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
| if (abs(gps_reg_x) > abs(gps_reg_y) ) |
| { |
| dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
| dev = gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
| dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
| } |
| else |