| 329,7 → 329,7 |
|---|
| //Richtung zur Home Positionbezogen auf Nordpol bestimmen |
| hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
| // in Winkel 0...360 Grad umrechnen |
| if ((-gps_rel_start_position.utm_east >= 0)) hdng_2home = ( 90-hdng_2home); |
| if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
| else hdng_2home = (270 - hdng_2home); |
| dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
| gps_state = GPS_CRTL_HOME_ACTIVE; |
| 415,11 → 415,12 |
|---|
| d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
| d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel |
| |
| if (d3 > GPS_G2T_DIST_MAX_STOP) |
| if (d3 > GPS_G2T_DIST_MAX_STOP) |
| { |
| hold_fast = 1; // Schnell Regeln |
| hold_reset_int = 1; // Integrator ausschalten |
| if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| { |
| hold_fast = 1; // Schnell Regeln |
| if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
| dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
| 426,18 → 427,21 |
|---|
| dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
| gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
| gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
| gps_sub_state = GPS_HOME_FAST_IN_TOL; |
| } |
| else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
| { |
| if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
| if (gps_g2t_act_v <= (GPS_G2T_V_MAX/2)) hold_fast = 0; // Wieder normal regeln |
| /* if (gps_sub_state == GPS_HOME_FAST_IN_TOL) hold_reset_int = 1; //Integrator einmal am Beginn des normalen Regelns resetten |
| else hold_reset_int = 0; |
| */ if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
| gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
| } |
| } |
| else //Schon ziemlich nahe am Ziel, deswegen abbremsen |
| { |
| hold_fast = 0; // Wieder normal regeln |
| if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| { |
| gps_sub_state = GPS_HOME_RMPDWN_IN_TOL; |
| if (d3 > GPS_G2T_DIST_HOLD) |
| { |
| if (gps_g2t_act_v < GPS_G2T_V_RAMP_DWN) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
| 449,15 → 453,23 |
|---|
| } |
| else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
| { |
| if (gps_rel_hold_position.utm_east > 1) gps_rel_hold_position.utm_east--; |
| else if (gps_rel_hold_position.utm_east <-1 ) gps_rel_hold_position.utm_east++; |
| if (gps_rel_hold_position.utm_north > 1) gps_rel_hold_position.utm_north--; |
| else if (gps_rel_hold_position.utm_north < -1 ) gps_rel_hold_position.utm_north++; |
| if ((abs(gps_rel_hold_position.utm_east) <= 1) && (abs(gps_rel_hold_position.utm_north) <=1)) gps_sub_state = GPS_HOME_FINISHED; |
| hold_fast = 0; // Wieder normal regeln |
| hold_reset_int = 0; // Integrator wieder aktivieren |
| if (gps_rel_hold_position.utm_east > GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN; |
| else if (gps_rel_hold_position.utm_east < -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN; |
| if (gps_rel_hold_position.utm_north > GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN; |
| else if (gps_rel_hold_position.utm_north < - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN; |
| if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN)) |
| { |
| gps_rel_hold_position.utm_east = 0; |
| gps_rel_hold_position.utm_north = 0; |
| gps_sub_state = GPS_HOME_FINISHED; |
| } |
| } |
| } |
| else |
| { |
| else |
| { |
| gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
| if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
| } |
| } |
| 514,7 → 526,7 |
|---|
| |
| diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10 |
| if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen |
| if (hold_fast > 0) diff_v = 10; //im schnellen Modus schwache Wirkung des Differenzierers |
| // if (hold_fast > 0) diff_v = 10; //im schnellen Modus schwache Wirkung des Differenzierers |
| |
| //I Werte begrenzen |
| #define INT1_MAX (20 * GPS_V) |
| 525,9 → 537,13 |
|---|
| if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
| else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
| |
| int diff_p; |
| if (hold_fast > 0) diff_p = 2; //im schnellen Modus Proportionalanteil staerken |
| else diff_p = 1; |
| |
| //PID Regler Werte aufsummieren |
| gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
| gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |
| gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
| gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |
| |
| //Ziel-Richtung bezogen auf Nordpol bestimmen |
| GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |