/branches/salvo_gps/GPS.c |
---|
329,7 → 329,7 |
//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); |
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; |
415,11 → 415,12 |
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) |
{ |
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 |
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); |
426,18 → 427,21 |
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 > 0) gps_g2t_act_v--; |
if (gps_g2t_act_v <= (GPS_G2T_V_MAX/2)) hold_fast = 0; // Wieder normal regeln |
/* 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--; |
gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
} |
} |
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 |
{ |
gps_sub_state = GPS_HOME_RMPDWN_IN_TOL; |
if (d3 > GPS_G2T_DIST_HOLD) |
{ |
if (gps_g2t_act_v < GPS_G2T_V_RAMP_DWN) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
449,15 → 453,23 |
} |
else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
{ |
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; |
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 |
{ |
else |
{ |
gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
} |
} |
514,7 → 526,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 schwache 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) |
525,9 → 537,13 |
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 = 2; //im schnellen Modus Proportionalanteil staerken |
else diff_p = 1; |
//PID Regler Werte aufsummieren |
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 |
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 |
//Ziel-Richtung bezogen auf Nordpol bestimmen |
GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |
/branches/salvo_gps/README_Gps.txt |
---|
2,12 → 2,11 |
GPS Implementierung von Peter Muehlenbrock ("Salvo") fürMikrokopter/FlightCrtl |
Stand 7.10.2007 |
Verwendung der SW ohne Gewaehr. Siehe auch die Lizenzbedingungen in den Files gps.c, math.c |
********************************************************************* |
********************************************************************* |
Hardware-Voraussetzungen: |
Kalibrierter Kompass vom Typ CMPS03, waagrecht eingebaut |
GPS Modul vom Typ ublox, Die Meldungungen "NAV_STATUS", "NAV_POSUTM" und "NAV_VELED" |
mussen mit 4 HZ Updaterate aktiviert sein. Anschluss an RX Port der FlightCRtl. Baudrate ist 57600 |
GPS Modul vom Typ ublox, Die Meldungungen "NAV_STATUS", "NAV_POSUTM" und "NAV_VELNED" |
mussen mit 4 HZ Updaterate aktiviert sein. Anschluss an RX Port der FlightCRtl. Baudrate ist 57600 wie beim Kopter Tool. |
Software-Voraussetzungen: |
in fc.h müsen die Neutralwerte ACC_X_NEUTRAL und ACC_Y_NEUTRAL |
27,7 → 26,8 |
Der GPS Hold Regler ist ein PID Regler, der ueber die UserParameter1(P), 2(I) und D(3) gesteuert wird. |
UserParameter1 beschreibt den P-Anteil, UserParameter2 den I-Anteil und UserParameter3 den D-Anteil. |
Hier kann und muss gespielt werden. |
Standardwerte sind 8 für den P-Anteil, 3 für den I-Anteil und 12 für den D-Anteil. |
Standardwerte bei wenig Wind sind 8 für den P-Anteil, 1 für den I-Anteil und 12 für den D-Anteil. |
Standardwerte bei rauheren verhältnissen sind 12 für den P-Anteil, 2 für den I-Anteil und 18 für den D-Anteil. |
Wenn alle 0 sind, ist der Regler deaktiviert. |
Voraussetzungen für GPS_Hold: |
49,11 → 49,11 |
der Magnetkompass in die waagrechte Lage kommt. |
Ein 3D Kompass ist damit überflüssig. |
GPS Komm heim zu Papi Funktion |
GPS Rücksturz zur Basis (GPS Home) Funktion |
Voraussetzungen wie bei GPS Hold. |
Die Funktion kann im Flug aktivert werden durch Setzen des Hoehenreglerschalters. |
Holgers Code habe ich so abgeändert daß der Höhenregler mit "Parameter_MaxHoehe" ab 50 aktiviert wird. |
Mit einem Schalter mit neutrallage können damit die Funktionen |
Mit einem Schalter mit Neutrallage können damit die Funktionen |
Alles aus - Hoehenregler ein, GPS Home Aus - Hoehenregler Ein UND GPS Home ein |
aktiviert werden. Ist noch nicht die beste Lösung und werde ich noch anpassen |
/branches/salvo_gps/fc.c |
---|
833,7 → 833,7 |
TimerWerteausgabe = 49; |
// DebugOut.Analog[0] = MesswertNick; |
// DebugOut.Analog[1] = MesswertRoll; |
DebugOut.Analog[0] = gps_state; |
DebugOut.Analog[0] = gps_sub_state; |
DebugOut.Analog[1] = dist_2home; |
DebugOut.Analog[2] = hdng_2home; |
/branches/salvo_gps/gps.h |
---|
71,7 → 71,7 |
extern GPS_ABS_POSITION_t gps_home_position; |
extern GPS_REL_POSITION_t gps_rel_act_position; |
extern GPS_REL_POSITION_t gps_rel_hold_position; |
extern short int gps_state; |
extern short int gps_state,gps_sub_state; |
extern signed int GPS_hdng_abs_2trgt; |
extern signed int GPS_hdng_rel_2trgt; |
extern signed int GPS_dist_2trgt; |
84,10 → 84,14 |
extern long int dist_flown; |
// Zustaende der zentralen GPS statemachine |
#define GPS_CRTL_IDLE 0 // |
#define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv |
#define GPS_CRTL_HOME_ACTIVE 2 // Rueckflug zur Basis Aktiv |
#define GPS_HOME_FINISHED 3 // Rueckflug zur Basis abgeschlossen |
#define GPS_CRTL_IDLE 0 // |
#define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv |
#define GPS_CRTL_HOME_ACTIVE 2 // Rueckflug zur Basis Aktiv |
#define GPS_HOME_FINISHED 3 // Rueckflug zur Basis abgeschlossen |
#define GPS_HOME_FAST_IN_TOL 4 // Rueckflug: Aktuelle Position innerhalb der Toleranz |
#define GPS_HOME_FAST_OUTOF_TOL 5 // Rueckflug: Aktuelle Position ausserhalb der Toleranz |
#define GPS_HOME_RMPDWN_IN_TOL 6 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz |
#define GPS_HOME_RMPDWN_OUTOF_TOL 7 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz |
// Kommandokonstanten fuer die zentrale GPS Statemachine |
#define GPS_CMD_REQ_INIT 0 // Initialisierung |
104,14 → 108,14 |
#define GPS_NICKROLL_MAX 40 // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
#define GPS_DIST_MAX 400 // Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm) |
#define GPS_STICK_HOLDOFF 25 // Wenn der Nick oder Roll Stickwerte groesser sind, wird GPS_HOLD deaktiviert |
#define GPS_V 8 // Teilerfaktor intern --> Ausgabewert |
#define GPS_V 8 // Teilerfaktor Regelabweichung zu Ausgabewert |
// GPS G2T /Go to Target Regler |
#define GPS_G2T_DIST_MAX_STOP 60 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm) |
#define GPS_G2T_DIST_HOLD 30 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler geschwindigkeit eingeregelt |
#define GPS_G2T_V_MAX 12 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird. |
#define GPS_G2T_V_RAMP_DWN 6 // Geschwindigkeit (in 10cm/0.25ekunden) in der Nahe der Home Position um abzubremsen |
#define GPS_G2T_V_MIN 3 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition. |
#define GPS_G2T_TOL 60 // Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |
#define GPS_G2T_DIST_MAX_STOP 60 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm) |
#define GPS_G2T_DIST_HOLD 30 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler geschwindigkeit eingeregelt |
#define GPS_G2T_V_MAX 8 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird. |
#define GPS_G2T_V_RAMP_DWN 4 // Geschwindigkeit (in 10cm/0.25ekunden) in der Nahe der Home Position um abzubremsen |
#define GPS_G2T_V_MIN 2 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition. |
#define GPS_G2T_TOL 50 // Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |