80,6 → 80,7 |
{ |
ublox_msg_state = UBLOX_IDLE; |
gps_state = GPS_CRTL_IDLE; |
gps_sub_state = GPS_CRTL_IDLE; |
actual_pos.status = 0; |
actual_speed.status = 0; |
actual_status.status = 0; |
332,7 → 333,7 |
// in Winkel 0...360 Grad umrechnen |
if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
else hdng_2home = (270 - hdng_2home); |
dist_2home = get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
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; |
return (GPS_STST_OK); |
} |
388,7 → 389,8 |
GPS_Roll = 0; |
gps_int_x = 0; |
gps_int_y = 0; |
gps_state = GPS_CRTL_IDLE; |
gps_sub_state = GPS_CRTL_IDLE; |
gps_state = GPS_CRTL_IDLE; |
return (GPS_STST_OK); |
break; |
|
418,43 → 420,43 |
|
if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
{ |
// hold_fast = 1; // Schnell Regeln |
hold_reset_int = 1; // Integrator ausschalten |
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) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
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); |
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 > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
} |
hold_fast = 1; // Regler fuer schnellen Flug |
hold_reset_int = 1; // Integrator ausschalten |
dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
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 |
} |
else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen |
{ |
hold_reset_int = 0; // Integrator einschalten |
if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) |
{ |
if (gps_g2t_act_v > GPS_G2T_V_RAMP_DWN) gps_g2t_act_v = GPS_G2T_V_RAMP_DWN; // Geschwindigkeit zu hoch |
else 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); |
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_RMPDWN_IN_TOL; |
dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit |
gps_sub_state = GPS_HOME_RMPDWN_IN_TOL; |
} |
else |
{ |
if (gps_g2t_act_v > 1) gps_g2t_act_v--; |
dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
} |
hold_fast = 0; // Wieder normal regeln |
hold_reset_int = 1; // Integrator ausschalten |
dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
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 |
} |
else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
{ |
474,6 → 476,7 |
gps_sub_state = GPS_HOME_FINISHED; |
} |
} |
else gps_sub_state = GPS_HOME_OUTOF_TOL; |
} |
} |
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
520,8 → 523,8 |
// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
signed long dist,int_east1,int_north1; |
int phi; |
phi = arctan_i(abs(dist_north),abs(dist_east)); |
dist = (long) (get_dist(dist_east,dist_north,phi)); //Zunaechst Entfernung zum Ziel ermitteln |
phi = arctan_i(abs(dist_north),abs(dist_east)); |
dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
|
if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer |
{ |
534,6 → 537,10 |
if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen |
} |
|
int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
else diff_p = GPS_PROP_NRML_V; |
|
//I Werte begrenzen |
#define INT1_MAX (20 * GPS_V) |
int_east1 = ((((long)int_east) * Parameter_UserParam2)/32); |
543,13 → 550,9 |
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 = 1; |
else diff_p = 2; //im schnellen Modus Proportionalanteil staerken |
|
//PID Regler Werte aufsummieren |
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 |
gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/(8*2))+ ((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*2))+ ((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); |