5,7 → 5,7 |
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License and GNU Lesser General Public License for more details. |
You should have received a copy of GNU General Public License ((License_GPL.txt) and |
You should have received a copy of GNU General Public License (License_GPL.txt) and |
GNU Lesser General Public License (License_LGPL.txt) along with this program. |
If not, see <http://www.gnu.org/licenses/>. |
|
16,7 → 16,7 |
Auswertung der Daten vom GPS im ublox Format |
Hold Modus mit PID Regler |
Rückstuerz zur Basis Funktion |
Stand 7.10.2007 |
Stand 8.10.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |
42,26 → 42,26 |
#define UBLOX_SYNCH1_CHAR 0xB5 |
#define UBLOX_SYNCH2_CHAR 0x62 |
|
signed int GPS_Nick = 0; |
signed int GPS_Roll = 0; |
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; //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 |
signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
signed int GPS_Nick = 0; |
signed int GPS_Roll = 0; |
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; //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 |
signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
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 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; |
signed int hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
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; |
|
short int Get_GPS_data(void); |
|
73,7 → 73,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 |
GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
|
// Initialisierung |
void GPS_Neutral(void) |
165,7 → 165,6 |
return (n); |
} |
|
|
/* |
Daten vom GPS im ublox MSG Format auswerten |
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen |
333,7 → 332,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 = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
dist_2home = 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); |
} |
417,72 → 416,72 |
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) // Schneller Rueckflug, noch weit weg vom Ziel |
{ |
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 |
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 |
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 |
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; |
gps_sub_state = GPS_HOME_FAST_IN_TOL; |
} |
else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
{ |
/* 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--; |
if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
} |
} |
else //Schon ziemlich nahe am Ziel, deswegen abbremsen |
else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen |
{ |
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
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 (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; |
if (d3 > GPS_G2T_DIST_HOLD) |
} |
else |
{ |
if (gps_g2t_act_v > 1) gps_g2t_act_v--; |
gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
} |
} |
else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
{ |
if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln |
{ |
gps_sub_state = GPS_HOME_IN_TOL; |
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)) |
{ |
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 |
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 = 0; |
gps_rel_hold_position.utm_north = 0; |
gps_sub_state = GPS_HOME_FINISHED; |
} |
else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
{ |
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 |
{ |
gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
} |
} |
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
return (GPS_STST_OK); |
} |
} |
} |
else return (GPS_STST_OK); |
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
return (GPS_STST_OK); |
} |
else |
else // Keine GPS Daten verfuegbar, deswegen Abbruch |
{ |
gps_state = GPS_CRTL_IDLE; //Abbruch |
gps_state = GPS_CRTL_IDLE; |
return (GPS_STST_ERR); |
} |
break; |
519,16 → 518,21 |
|
// 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 |
#define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm |
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 |
dist = (long) (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 |
// if (hold_fast > 0) diff_v = 10; //im schnellen Modus schwache Wirkung des Differenzierers |
if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer |
{ |
diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10 |
if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen |
} |
else |
{ |
diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10 |
if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen |
} |
|
//I Werte begrenzen |
#define INT1_MAX (20 * GPS_V) |
540,8 → 544,8 |
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; |
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 |