| 370,11 → 370,11 |
|---|
| { |
| hold_fast = 0; |
| 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; |
| speed_east = 0; speed_north= 0; |
| int_ovfl_cnt = 0; |
| int_east = 0, int_north = 0; |
| gps_reg_x = 0, gps_reg_y = 0; |
| dist_east = 0, dist_north = 0; |
| speed_east = 0; speed_north= 0; |
| int_ovfl_cnt = 0; |
| gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
| gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
| gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
| 383,7 → 383,7 |
|---|
| } |
| else |
| { |
| gps_rel_hold_position.status = 0; //Keine Daten verfuegbar |
| gps_rel_hold_position.status = 0; //Keine Daten verfuegbar |
| gps_state = GPS_CRTL_IDLE; |
| return(GPS_STST_ERR); // Keine Daten da |
| } |
| 435,7 → 435,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 langsam erhoehen |
| if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit flott erhoehen |
| dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| gps_sub_state = GPS_HOME_FAST_IN_TOL; |
| } |
| 486,7 → 486,7 |
|---|
| { |
| gps_rel_hold_position.utm_east = 0; |
| gps_rel_hold_position.utm_north = 0; |
| gps_sub_state = GPS_HOME_FINISHED; |
| gps_sub_state = GPS_HOME_FINISHED; |
| } |
| } |
| else gps_sub_state = GPS_HOME_OUTOF_TOL; |
| 532,18 → 532,25 |
|---|
| int_east = 0; |
| int_north = 0; |
| } |
| if (hold_fast > 0) //schneller Coming Home Modus Einfluss des D-Anteils verkleinern |
| if (hold_fast > 0) //schneller Coming Home Modus |
| { |
| amplfy_speed_east = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| amplfy_speed_north = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| // Verstaerkungsfaktor fuer D-Anteil ermitteln um exponentielles Verhalten zu erzielen |
| amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_F_MAX-1)*10)/DIFF_X_F_MAX) +10; |
| amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_F_MAX-1)*10)/DIFF_X_F_MAX) +10; |
| if (amplfy_speed_east > (DIFF_Y_F_MAX *10)) amplfy_speed_east = DIFF_Y_F_MAX *10; // Begrenzung |
| 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 |
| { |
| // Verstaerkungsfaktor fuer Geschwindigkeit ermitteln um exponentielles Verhalten zu erzielen |
| amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10; |
| amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10; |
| if (amplfy_speed_east > (DIFF_Y_MAX *10)) amplfy_speed_east = DIFF_Y_MAX *10; // Begrenzung |
| if (amplfy_speed_north > (DIFF_Y_MAX *10)) amplfy_speed_north = DIFF_Y_MAX *10; // Begrenzung |
| // Verstaerkungsfaktor fuer D-Anteil ermitteln um exponentielles Verhalten zu erzielen |
| amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
| amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
| if (amplfy_speed_east > (DIFF_Y_N_MAX *10)) amplfy_speed_east = DIFF_Y_N_MAX *10; // Begrenzung |
| 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 |
| 562,7 → 569,7 |
|---|
| //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; |
| int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; //Fehler behoben am 17.12.2007 vorher int_north= |
| int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
| if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
| else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
| if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
| 611,11 → 618,11 |
|---|
| n = n/GPS_V; |
| n1 = n1/GPS_V; |
| //Kleine Werte verstaerken, Grosse abschwaechen |
| /* n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
| /* n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
| GPS_Roll = (int) n_l; |
| n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
| n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
| GPS_Nick = (int) n_l; |
| */ |
| |