| /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 |