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); |