13,7 → 13,7 |
Peter Muehlenbrock |
Auswertung der Daten vom GPS im ublox Format |
Hold Modus mit PID Regler |
Komm heim zu Papi Funktion |
Rückstuerz zur Basis Funktion |
Stand 6.10.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
56,7 → 56,7 |
unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
static short int hold_fast; //Flag fuer Hold Regler |
static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
static uint8_t *ptr_payload_data; |
static uint8_t *ptr_pac_status; |
long int dist_flown; |
286,12 → 286,13 |
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
short int GPS_CRTL(short int cmd) |
{ |
static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
static signed int int_east,int_north; //Integrierer |
static signed int dist_north,dist_east; |
static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
signed int n,diff_v; |
static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
long signed int dev,n_l; |
signed int dist_frm_start_east,dist_frm_start_north; |
switch (cmd) |
307,6 → 308,7 |
cnt = 0; |
gps_tick = 0; |
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; |
313,6 → 315,7 |
diff_east_f = 0, diff_north_f= 0; |
diff_east = 0, diff_north = 0; |
dist_flown = 0; |
gps_g2t_act_v = 0; |
gps_sub_state = GPS_CRTL_IDLE; |
// aktuelle positionsdaten abspeichern |
if (gps_rel_act_position.status > 0) |
354,7 → 357,8 |
// aktuelle positionsdaten abspeichern |
if (gps_rel_act_position.status > 0) |
{ |
hold_fast = 0; |
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; |
416,22 → 420,28 |
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
{ |
hold_fast = 1; // Schnell Regeln |
dist_flown += (GPS_G2T_V_MAX); // Vorgabe der Strecke anhand der Geschwindigkeit |
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 |
} |
else hold_fast = 0; // Wieder normal regeln |
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 |
} |
} |
else //Schon ziemlich nahe am Ziel, deswegen abbremsen |
{ |
hold_fast = 0; // Wieder normal regeln |
hold_fast = 0; // Wieder normal regeln |
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
{ |
if (d3 > GPS_G2T_DIST_HOLD) |
{ |
dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit |
if (gps_g2t_act_v < GPS_G2T_V_RAMP_DWN) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000; |
dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000; |
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
445,7 → 455,11 |
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; |
} |
} |
} |
else |
{ |
if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
} |
} |
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
return (GPS_STST_OK); |
460,7 → 474,7 |
break; |
|
|
case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
{ |
gps_updte_flag = 0; |
477,13 → 491,13 |
diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern |
diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern |
*/ |
#define GPSINT_MAX 2048 //neuer Wert ab 1.10.2007 Begrenzung |
#define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung |
if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
{ |
int_east -= dist_east; |
int_north -= dist_north; |
} |
if (hold_fast > 0) //Im Schnellen Mode Integrator abschalten |
if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
{ |
int_east = 0; |
int_north = 0; |
493,7 → 507,7 |
// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
#define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10 |
#define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm |
signed long dist; |
signed long dist,int_east1,int_north1; |
int phi; |
phi = arctan_i(abs(dist_north),abs(dist_east)); |
dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
500,11 → 514,20 |
|
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 keine staerkere 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) |
int_east1 = ((((long)int_east) * Parameter_UserParam2)/32); |
int_north = ((((long)int_north) * Parameter_UserParam2)/32); |
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 |
else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
|
//PID Regler Werte aufsummieren |
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse |
gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((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)/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 |
|
//Ziel-Richtung bezogen auf Nordpol bestimmen |
GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |
536,7 → 559,6 |
GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
|
#define GPS_V 8 // auf Maximalwert normieren. Teilerfaktor ist 8 |
if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |