434,8 → 434,8 |
} |
else 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/2) gps_g2t_act_v -= 1; //Geschwindigkeit auf Haeflte runter oder rauffahren |
else if (gps_g2t_act_v < GPS_G2T_V_MAX/2) gps_g2t_act_v += 1; |
if (gps_g2t_act_v > GPS_G2T_V_MAX/2) gps_g2t_act_v -= 1; //Geschwindigkeit auf Haaelte runter oder rauffahren |
else if (gps_g2t_act_v < GPS_G2T_V_MAX/2) gps_g2t_act_v += 1; //Geschwindigkeit erhoehen |
dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
gps_sub_state = GPS_HOME_FAST_IN_TOL; |
} |
445,7 → 445,7 |
// dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
} |
hold_reset_int = 1; // Integrator aussschalten |
hold_reset_int = 0; // Integrator einsschalten |
hold_fast = 1; // Regler fuer schnellen Flug |
dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
519,14 → 519,10 |
dist_north = (int)delta_north; |
|
|
// #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
long int gpsintmax; |
if (Parameter_UserParam2 > 0) |
#define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
{ |
gpsintmax = (GPS_NICKROLL_MAX * GPS_V * GPS_USR_PAR_FKT * ((32*4)/10))/(long)Parameter_UserParam2; //auf ungefeahren Maximalwert begrenzen |
if ((abs(int_east) > (int)gpsintmax) || (abs(int_north)> (int)gpsintmax)) |
{ |
int_ovfl_cnt += 1; // Zahl der Overflows zaehlen |
if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen |
} |
if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren |
{ |
540,18 → 536,9 |
int_east = 0; |
int_north = 0; |
} |
} |
else // Integrator deaktiviert |
{ |
int_east = 0; |
int_north = 0; |
} |
|
debug_gp_4 = (int)int_east; // zum Debuggen |
debug_gp_5 = (int)int_north; // zum Debuggen |
|
//I Werte begrenzen |
#define INT1_MAX (GPS_NICKROLL_MAX * GPS_V*2)/10// auf 20 Prozent des maximalen Nick/Rollwert begrenzen |
#define INT1_MAX (GPS_NICKROLL_MAX * GPS_V*5)/10// auf 40 Prozent des maximalen Nick/Rollwert begrenzen |
int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
565,8 → 552,8 |
amplfy_speed_north = DIFF_Y_F_MAX; |
amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
speed_east = (speed_east * (long)amplfy_speed_east) /50; |
speed_north = (speed_north * (long)amplfy_speed_north)/50; |
speed_east = (speed_east * (long)amplfy_speed_east) /100; |
speed_north = (speed_north * (long)amplfy_speed_north)/100; |
// D Werte begrenzen |
#define D_F_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
if (speed_east > D_F_MAX) speed_east = D_F_MAX; |