13,7 → 13,8 |
Peter Muehlenbrock |
Auswertung der Daten vom GPS im ublox Format |
Hold Modus mit PID Regler |
Stand 2.10.2007 |
Komm heim zu Papi Funktion |
Stand 5.10.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |
44,7 → 45,7 |
short int ublox_msg_state = UBLOX_IDLE; |
static uint8_t chk_a =0; //Checksum |
static uint8_t chk_b =0; |
short int gps_state; |
short int gps_state,gps_sub_state; |
short int gps_updte_flag; |
signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol |
signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
53,10 → 54,14 |
static unsigned int rx_len; |
static unsigned int ptr_payload_data_end; |
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 uint8_t *ptr_payload_data; |
static uint8_t *ptr_pac_status; |
|
long int dist_flown,dist_frm_start_east,dist_frm_start_north; |
|
short int Get_GPS_data(void); |
|
NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt |
67,6 → 72,7 |
GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt |
GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position |
GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode |
GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
|
// Initialisierung |
void GPS_Neutral(void) |
109,7 → 115,7 |
short int n = 0; |
n = Get_GPS_data(); |
if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind |
if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in MotorreglerRoutine ueberwacht und dekrementiert |
if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert |
if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt |
{ |
gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east); |
288,12 → 294,53 |
static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
signed int n,diff_v; |
long signed int dev,n_l; |
|
switch (cmd) |
{ |
|
case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
// Noch einiges zu ueberlegen und zu tun |
return(GPS_STST_PEND); // noch warten |
if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE)) |
{ |
cnt++; |
if (cnt > 500) // erst nach Verzoegerung |
{ |
// Erst mal initialisieren |
cnt = 0; |
gps_tick = 0; |
int_east = 0, int_north = 0; |
gps_reg_x = 0, gps_reg_y = 0; |
dist_east = 0, dist_north = 0; |
diff_east_f = 0, diff_north_f= 0; |
diff_east = 0, diff_north = 0; |
dist_flown = 0; |
gps_sub_state = GPS_CRTL_IDLE; |
// aktuelle positionsdaten abspeichern |
if (gps_rel_act_position.status > 0) |
{ |
gps_rel_start_position.utm_east = gps_rel_act_position.utm_east; |
gps_rel_start_position.utm_north= gps_rel_act_position.utm_north; |
gps_rel_start_position.status = 1; // gueltige Positionsdaten |
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 |
//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); |
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; |
return (GPS_STST_OK); |
} |
else |
{ |
gps_rel_start_position.status = 0; //Keine Daten verfuegbar |
gps_state = GPS_CRTL_IDLE; |
return(GPS_STST_ERR); // Keine Daten da |
} |
} |
else return(GPS_STST_PEND); // noch warten |
} |
break; |
// ****************************** |
|
304,19 → 351,14 |
if (cnt > 500) // erst nach Verzoegerung |
{ |
cnt = 0; |
// aktuelle positionsdaten abespeichern |
// aktuelle positionsdaten abspeichern |
if (gps_rel_act_position.status > 0) |
{ |
int_east = 0; |
int_north = 0; |
gps_reg_x = 0; |
gps_reg_y = 0; |
dist_east = 0; |
dist_north = 0; |
diff_east_f = 0; |
diff_north_f = 0; |
diff_east = 0; |
diff_north = 0; |
int_east = 0, int_north = 0; |
gps_reg_x = 0, gps_reg_y = 0; |
dist_east = 0, dist_north = 0; |
diff_east_f = 0, diff_north_f= 0; |
diff_east = 0, diff_north = 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 |
356,8 → 398,65 |
return (GPS_STST_OK); |
break; |
|
case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis |
//Der Sollwert des Lagereglers wird der Homeposition angenaehert |
if (gps_rel_start_position.status >0) |
{ |
if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist |
{ |
gps_tick++; |
int d1,d2,d3; |
d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel |
d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
|
if (d3 > GPS_G2T_DIST_MAX_STOP) |
{ |
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
{ |
dist_flown += (GPS_G2T_V_MAX); // 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_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt |
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
} |
} |
else //Schon ziemlich nahe am Ziel, deswegen abbremsen |
{ |
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
{ |
if (d3 > 0) |
{ |
dist_flown += GPS_G2T_V_RAMP_DWN; // 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_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt |
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
} |
else //Ziel erreicht, Regelung beenden |
{ |
gps_rel_hold_position.utm_east = 0; |
gps_rel_hold_position.utm_north = 0; |
gps_sub_state = GPS_HOME_FINISHED; |
} |
} |
|
} |
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
return (GPS_STST_OK); |
} |
else return (GPS_STST_OK); |
} |
else |
{ |
gps_state = GPS_CRTL_IDLE; //Abbruch |
return (GPS_STST_ERR); |
} |
break; |
|
|
case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen |
if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
{ |
gps_updte_flag = 0; |
// ab hier wird geregelt |
369,10 → 468,10 |
int_north += dist_north; |
diff_east += dist_east; // Differenz zur vorhergehenden East Position |
diff_north += dist_north; // Differenz zur vorhergehenden North Position |
|
/* |
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 |
if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
{ |
386,22 → 485,14 |
signed long dist; |
int phi; |
phi = arctan_i(abs(dist_north),abs(dist_east)); |
if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln |
{ |
dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen |
dist = abs((dist *1000) / (long) sin_i(phi)); |
} |
else |
{ |
dist = (long)dist_north; |
dist = abs((dist *1000) / (long) cos_i(phi)); |
} |
dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
|
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 |
|
//PID Regler Werte aufsummieren |
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east_f * 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_f * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil Y Achse |
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 |
|
//Ziel-Richtung bezogen auf Nordpol bestimmen |
GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |
455,12 → 546,21 |
return (GPS_STST_ERR); |
break; |
} |
else |
{ |
if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen |
return (GPS_STST_OK); |
} |
} |
else return (GPS_STST_OK); |
else |
{ |
if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen |
return (GPS_STST_OK); |
} |
break; |
|
default: |
gps_state = GPS_CRTL_IDLE; |
gps_state = GPS_CRTL_IDLE; |
return (GPS_STST_ERR); |
break; |
} |