| 297,8 → 297,8 |
|---|
| //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
| short int GPS_CRTL(short int cmd) |
| { |
| static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| static signed int dist_north,dist_east; // Distanz zur Sollpoistion |
| static unsigned int cnt; // Zaehler fuer diverse Verzoegerungen |
| static long int delta_north,delta_east; // Mass fuer Distanz zur Sollposition |
| signed int n,n1; |
| static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
| long signed int dev; |
| 307,8 → 307,10 |
|---|
| static signed int int_east,int_north; //Integrierer |
| long int speed_east,speed_north; //Aktuelle Geschwindigkeit |
| signed long int_east1,int_north1; |
| // signed long dist; |
| int dist_east,dist_north; |
| int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
| |
| |
| switch (cmd) |
| { |
| |
| 325,7 → 327,7 |
|---|
| hold_reset_int = 0; // Integrator enablen |
| int_east = 0, int_north = 0; |
| gps_reg_x = 0, gps_reg_y = 0; |
| dist_east = 0, dist_north = 0; |
| delta_east = 0, delta_north = 0; |
| dist_flown = 0; |
| gps_g2t_act_v = 0; |
| gps_sub_state = GPS_CRTL_IDLE; |
| 373,7 → 375,7 |
|---|
| hold_reset_int = 0; // Integrator enablen |
| int_east = 0, int_north = 0; |
| gps_reg_x = 0, gps_reg_y = 0; |
| dist_east = 0, dist_north = 0; |
| delta_east = 0, delta_north = 0; |
| speed_east = 0; speed_north= 0; |
| int_ovfl_cnt = 0; |
| gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
| 436,7 → 438,7 |
|---|
| { |
| if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| { |
| if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit flott erhoehen |
| if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit erhoehen |
| dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| gps_sub_state = GPS_HOME_FAST_IN_TOL; |
| } |
| 508,15 → 510,15 |
|---|
| if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
| { |
| // ab hier wird geregelt |
| 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; |
| delta_east = (long) (gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east); |
| delta_north = (long) (gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north); |
| int_east += (int)delta_east; |
| int_north += (int)delta_north; |
| 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 |
| dist_east = (int)delta_east; //merken |
| dist_north = (int)delta_north; |
| |
| |
| #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
| 527,7 → 529,7 |
|---|
| if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren |
| { |
| int_ovfl_cnt -= 1; |
| int_east = (int_east*7)/8; // Wert reduzieren |
| int_east = (int_east*7)/8; // Wert reduzieren |
| int_north = (int_north*7)/8; |
| } |
| |
| 545,6 → 547,9 |
|---|
| 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); |
| speed_east = speed_east * abs(speed_east) /200; //quadrieren |
| speed_north = speed_north * abs(speed_north) /200; //quadrieren |
| diff_p = (Parameter_UserParam1 * GPS_PROP_FAST_V)/GPS_USR_PAR_FKT; //Verstaerkung fuer P-Anteil |
| } |
| else //langsamer Holdmodus |
| { |
| 555,18 → 560,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); |
| speed_east = speed_east * abs(speed_east) /100; //quadrieren |
| speed_east = speed_east * abs(speed_east) /100; //quadrieren |
| speed_north = speed_north * abs(speed_north) /100; //quadrieren |
| diff_p = (Parameter_UserParam1 * GPS_PROP_NRML_V)/GPS_USR_PAR_FKT; //Verstaerkung fuer P-Anteil |
| |
| } |
| // amplfy_speed_east /= 10; |
| // amplfy_speed_north /= 10; |
| debug_gp_4 = (int)speed_east; // zum Debuggen |
| debug_gp_5 = (int)speed_north; // zum Debuggen |
| |
| 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; |
| |
| //I Werte begrenzen |
| #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V)/2 //ab 30.12.2007 auf Halben Maximalen Nick/Rollwert begrenzen |
| int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| 576,10 → 577,16 |
|---|
| if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
| else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
| |
| //P-Werte quadrieren |
| delta_east = (abs(delta_east) * delta_east )/20; //quadrieren |
| delta_north = (abs(delta_north) * delta_north)/20; //quadrieren |
| debug_gp_2 = (int)delta_east; // zum Debuggen |
| debug_gp_3 = (int)delta_north; // zum Debuggen |
| |
| //PID Regler Werte aufsummieren |
| 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 |
| gps_reg_x = -(int_east1 + ((delta_east * (long)diff_p)/(8*2))+ ((speed_east * (long)amplfy_speed_east )/(100L))); // I + P +D Anteil X Achse |
| gps_reg_y = -(int_north1 + ((delta_north * (long)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 |