| 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 Haaelte runter oder rauffahren |
| else if (gps_g2t_act_v < GPS_G2T_V_MAX/2) gps_g2t_act_v += 1; //Geschwindigkeit erhoehen |
| 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; |
| 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 = 0; // Integrator einsschalten |
| hold_reset_int = 1; // Integrator aussschalten |
| 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,26 → 519,39 |
|---|
| dist_north = (int)delta_north; |
| |
| |
| #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
| if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
| // #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
| long int gpsintmax; |
| if (Parameter_UserParam2 > 0) |
| { |
| if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen |
| 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 > 0) //bei Overflow Wert Integratorwert reduzieren |
| { |
| int_ovfl_cnt -= 1; |
| int_east = (int_east*7)/8; // Wert reduzieren |
| int_north = (int_north*7)/8; |
| } |
| |
| if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
| { |
| int_east = 0; |
| int_north = 0; |
| } |
| } |
| if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren |
| else // Integrator deaktiviert |
| { |
| int_ovfl_cnt -= 1; |
| int_east = (int_east*7)/8; // Wert reduzieren |
| int_north = (int_north*7)/8; |
| int_east = 0; |
| int_north = 0; |
| } |
| |
| if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
| { |
| 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*5)/10// auf 40 Prozent des maximalen Nick/Rollwert begrenzen |
| #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V*2)/10// auf 20 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 |
| 552,8 → 565,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) /100; |
| speed_north = (speed_north * (long)amplfy_speed_north)/100; |
| speed_east = (speed_east * (long)amplfy_speed_east) /50; |
| speed_north = (speed_north * (long)amplfy_speed_north)/50; |
| // 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; |