14,7 → 14,7 |
Auswertung der Daten vom GPS im ublox Format |
Hold Modus mit PID Regler |
Komm heim zu Papi Funktion |
Stand 5.10.2007 |
Stand 6.10.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |
45,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,gps_sub_state; |
short int gps_state,gps_sub_state; //Zustaende der Statemachine |
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 |
56,12 → 56,11 |
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 uint8_t *ptr_payload_data; |
static uint8_t *ptr_pac_status; |
long int dist_flown; |
|
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 |
294,7 → 293,7 |
static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
signed int n,diff_v; |
long signed int dev,n_l; |
|
signed int dist_frm_start_east,dist_frm_start_north; |
switch (cmd) |
{ |
|
307,6 → 306,7 |
// Erst mal initialisieren |
cnt = 0; |
gps_tick = 0; |
hold_fast = 0; |
int_east = 0, int_north = 0; |
gps_reg_x = 0, gps_reg_y = 0; |
dist_east = 0, dist_north = 0; |
354,6 → 354,7 |
// aktuelle positionsdaten abspeichern |
if (gps_rel_act_position.status > 0) |
{ |
hold_fast = 0; |
int_east = 0, int_north = 0; |
gps_reg_x = 0, gps_reg_y = 0; |
dist_east = 0, dist_north = 0; |
406,41 → 407,45 |
{ |
gps_tick++; |
int d1,d2,d3; |
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 ); |
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 |
{ |
{ |
hold_fast = 1; // Schnell Regeln |
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 |
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 //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 |
{ |
if (d3 > 0) |
if (d3 > GPS_G2T_DIST_HOLD) |
{ |
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_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
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 |
else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
{ |
gps_rel_hold_position.utm_east = 0; |
gps_rel_hold_position.utm_north = 0; |
gps_sub_state = GPS_HOME_FINISHED; |
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; |
} |
} |
|
} |
} |
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
return (GPS_STST_OK); |
478,6 → 483,12 |
int_east -= dist_east; |
int_north -= dist_north; |
} |
if (hold_fast > 0) //Im Schnellen Mode Integrator abschalten |
{ |
int_east = 0; |
int_north = 0; |
} |
|
// Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
// 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 |
489,6 → 500,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 keine staerkere Wirkung des Differenzierers |
|
//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 |