| /branches/salvo_gps/GPS.c |
|---|
| 283,8 → 283,10 |
| //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
| short int GPS_CRTL(short int cmd) |
| { |
| static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| static int int_east,int_north; //Integrierer |
| static signed int dist_north,dist_east; |
| int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
| int n; |
| long int dist; |
| switch (cmd) |
| 349,18 → 351,22 |
| return (GPS_STST_OK); |
| break; |
| case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
| 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 |
| { |
| gps_updte_flag = 0; |
| // ab hier wird geregelt |
| signed int dist_north,dist_east; |
| diff_east = -dist_east; //Alten Wert schon mal abziehen |
| diff_north = -dist_north; |
| dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
| dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| int_east += dist_east; |
| int_north += dist_north; |
| diff_east += dist_east; // Differenz zur vorhergehenden East Position |
| diff_north += dist_north; // Differenz zur vorhergehenden North Position |
| #define GPSINT_MAX 1000 |
| #define GPSINT_MAX 2048 //neuer Wert ab 25.9.2007 Begrenzung |
| if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
| { |
| int_east -= dist_east; |
| 367,15 → 373,15 |
| int_north -= dist_north; |
| } |
| #define DIST_MAX 200 //neu ab 19.9. 1900 Begrenzung |
| #define DIST_MAX 100 //neuer Wert ab 24.9.2007 Begrenzung |
| if (dist_east > DIST_MAX) dist_east = DIST_MAX; |
| if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
| if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
| if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
| //PI Regler |
| gps_reg_x = ((int_east * Parameter_UserParam1)/16) + ((dist_east * Parameter_UserParam2)/8); // P+I Anteil |
| gps_reg_y = ((int_north * Parameter_UserParam1)/16) + ((dist_north * Parameter_UserParam2)/8); // P+I Anteil |
| //PID Regler |
| gps_reg_x = ((int_east * Parameter_UserParam1)/32) + ((dist_east * Parameter_UserParam2)/2)+ ((diff_east * Parameter_UserParam3)/2); // |
| gps_reg_y = ((int_north * Parameter_UserParam1)/32) + ((dist_north * Parameter_UserParam2)/2)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
| //Richtung bezogen auf Nordpol bestimmen |
| GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
| 387,7 → 393,7 |
| // Relative Richtung in bezug auf Nordachse des Kopters errechen |
| n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
| GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n; |
| if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180))GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360; |
| if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360; |
| else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt; |
| else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
| /branches/salvo_gps/README_Gps.txt |
|---|
| 1,6 → 1,6 |
| ********************************************************************* |
| GPS Implementierung von Peter Muehlenbrock ("Salvo") fuer Mikrokopter/FlightCrtl |
| Stand 19.9.2007 |
| Stand 25.9.2007 |
| Verwendung der SW ohne Gewaehr. Siehe auch die Lizenzbedingungen in den Files gps.c, math.c |
| ********************************************************************* |
| 26,16 → 26,16 |
| Alle Einstellungen beziehen sich auf AVR Studio von Atmel und den WIN_AVR Compiler. |
| Parametrierung: |
| DER GPS Hold Regler ist ein PI Regler, der ueber die UserParameter1 und 2 gesteuert wird. |
| UserParameter1 beschreibt den I-Anteil, UserParameter2 den P Anteil. |
| Hier kann gespielt werden. Wenn beide 0 sind, ist der Regler deaktiviert. |
| Standardwerte sind 16 fuer Parameter1 und 64 fuer Parameter 2. |
| Der GPS Hold Regler ist ein PID Regler, der ueber die UserParameter1(I), 2(P) und D(3) gesteuert wird. |
| UserParameter1 beschreibt den I-Anteil, UserParameter2 den P Antei, UserParameter3 den D Anteil. |
| Hier kann gespielt werden. Wenn alle 0 sind, ist der Regler deaktiviert. |
| Standardwerte sind 1 fuer Parameter1, 32 fuer Parameter 2 und 4 fuer Parameter 3. |
| Voraussetzungen für GPS_Hold: |
| Neben den genannten HW und SW Voraussetzungen muß beim Kalibrieren das GPS MOdul bereits Positions |
| daten liefern. Nur dann wird die Home Position abgespeichert und nur dann wird GPS_Hold aktiviert. |
| Kenntlich gemacht wird dies durch einen etwas laengeren Piepser nach dem Kalibrieren. |
| Das GPS Aktiv Flag im Setting muss aktiviert sein. Eine Ausrichtung des Kopters nach Nord etc ist nicht erforderlich. |
| Das GPS Aktiv Flag im Setting muss aktiviert sein. Eine Ausrichtung des Kopters nach Nord etc. ist nicht erforderlich. |
| Aktivierung GPS_Hold im Flug: |
| Wenn alle genannten Voraussetzungen erfüllt sind, wird GPS Hold im Flug automatisch aktiviert, sobald der Nick und Roll Stick |
| 45,7 → 45,7 |
| GPS_Hold regelt nur horizontale Abweichungen, keine Hoehe. |
| Der Magnetkompass wird nur in waagrechter Lage dazu benutzt einen |
| internen aus dem Giergyro ermittelten "Ersatz"kompasswert nach zu kalibrieren. Daher funktioniert die |
| bestimmung der Kopterausrichtung in jeder Lebenslage, sofern nur ab und zu der Magnetkompass in die waagrechte Lage kommt. |
| Bstimmung der Kopterausrichtung in jeder Lebenslage, sofern nur ab und zu der Magnetkompass in die waagrechte Lage kommt. |
| Ein 3D Kompass ist damit überfluessig. |
| Bekannte Maengel: |
| /branches/salvo_gps/fc.c |
|---|
| 317,7 → 317,7 |
| EE_Parameter.Stick_P = 2; //2 // Wert : 1-6 |
| EE_Parameter.Stick_D = 4; //8 // Wert : 0-64 |
| EE_Parameter.Gier_P = 16; // Wert : 1-20 |
| EE_Parameter.Gas_Min = 20; // Wert : 0-32 |
| EE_Parameter.Gas_Min = 15; // Wert : 0-32 |
| EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
| EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
| EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
| 360,7 → 360,7 |
| EE_Parameter.Stick_P = 2; //2 // Wert : 1-6 |
| EE_Parameter.Stick_D = 0; //8 // Wert : 0-64 |
| EE_Parameter.Gier_P = 16; // Wert : 1-20 |
| EE_Parameter.Gas_Min = 20; // Wert : 0-32 |
| EE_Parameter.Gas_Min = 15; // Wert : 0-32 |
| EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
| EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
| EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
| /branches/salvo_gps/fc.h |
|---|
| 24,7 → 24,7 |
| extern unsigned char Parameter_UserParam1 ; |
| extern unsigned char Parameter_UserParam2 ; |
| extern unsigned char Parameter_UserParam3 ; |
| // Salvo End |
| extern volatile unsigned char Timeout; |
| extern unsigned char Sekunde,Minute; |
| /branches/salvo_gps/gps.h |
|---|
| 96,5 → 96,5 |
| #define GPS_STST_ERR 2 // Fehler |
| // GPS Lageregler |
| #define GPS_NICKROLL_MAX 15 //Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
| #define GPS_NICKROLL_MAX 20 //Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |