/branches/salvo_gps/GPS.c |
---|
375,10 → 375,10 |
diff_east += dist_east; // Differenz zur vorhergehenden East Position |
diff_north += dist_north; // Differenz zur vorhergehenden North Position |
diff_east_f = ((diff_east_f )/2) + (diff_east /2); //Differenzierer filtern |
diff_north_f = ((diff_north_f)/2) + (diff_north/2); //Differenzierer filtern |
diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern |
diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern |
#define GPSINT_MAX 2048 //neuer Wert ab 25.9.2007 Begrenzung |
#define GPSINT_MAX 2048 //neuer Wert ab 1.10.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; |
402,11 → 402,11 |
dist_north_p = (int) d; |
//PID Regler Werte aufsummieren |
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east_p * Parameter_UserParam1)/4)+ ((diff_east_f * Parameter_UserParam3)/2); // I + P +D Anteil X Achse |
gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north_p * Parameter_UserParam1)/4)+ ((diff_north_f * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east_p * Parameter_UserParam1)/8)+ ((diff_east_f * Parameter_UserParam3)); // I + P +D Anteil X Achse |
gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north_p * Parameter_UserParam1)/8)+ ((diff_north_f * Parameter_UserParam3)); // I + P +D Anteil Y Achse |
//Ziel-Richtung bezogen auf Nordpol bestimmen |
GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |
// in Winkel 0...360 Grad umrechnen |
if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
435,7 → 435,7 |
GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
#define GPS_V 8 |
#define GPS_V 8 // auf Maximalwert normieren. Teilerfaktor ist 8 |
if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |
/branches/salvo_gps/README_Gps.txt |
---|
1,6 → 1,6 |
********************************************************************* |
GPS Implementierung von Peter Muehlenbrock ("Salvo") fürMikrokopter/FlightCrtl |
Stand 29.9.2007 |
Stand 1.10.2007 |
Verwendung der SW ohne Gewaehr. Siehe auch die Lizenzbedingungen in den Files gps.c, math.c |
********************************************************************* |
27,18 → 27,19 |
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. Wenn alle 0 sind, ist der Regler deaktiviert. |
Standardwerte sind 8 für den P-Anteil, 1 für den I-Anteil und 12 für den D-Anteil. |
Standardwerte sind 8 für den P-Anteil, 2 für den I-Anteil und 10 für den D-Anteil. |
Voraussetzungen für GPS_Hold: |
Neben den genannten HW und SW Voraussetzungen muß beim Kalibrieren das GPS MOdul bereits Positionsdaten liefern. |
Nur dann wird die Home Position abgespeichert und nur dann wird GPS_Hold aktiviert. |
Kenntlich gemacht wird dies durch einen etwas längeren 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 |
fürca. 500msec in Neutrallage sind. Weicht einer der Sticks davon ab (Parameter GPS_STICK_HOLDOFF) oder liefert das GPS Modul keine |
gueltigen Daten mehr wird GPS_Hold sofort deaktiviert. |
fürca. 500msec in Neutrallage sind. Weicht einer der Sticks davon ab (Parameter GPS_STICK_HOLDOFF) oder liefert das GPS Modul oder fällt der |
Kompass aus wird GPS_Hold sofort deaktiviert. |
Damit kann jederzeit die manuelle Kontrolle wieder übernommen werden. |
GPS_Hold regelt nur horizontale Abweichungen, keine Hoehe. |
47,8 → 48,8 |
der Magnetkompass in die waagrechte Lage kommt. |
Ein 3D Kompass ist damit überflüssig. |
Bekannte Maengel: |
Leichte Schwingneigung, bei längerer Neigung weicht der Ersatzkompass ab. |
Bekannte Schwächen: |
Leichte Schwingneigung ca. +-5m, bei längerer Neigung weicht der Ersatzkompass ab. |
PID Regler muss noch besser parametriert werden. |
/branches/salvo_gps/math.c |
---|
14,7 → 14,7 |
Winkelfunktionen sin, cos und arctan in |
brute-force Art: Sehr Schnell, nicht sonderlich genau, aber ausreichend |
Sinus Funktion von Nick666 vereinfacht |
Stand 30.9.2007 |
Stand 1.10.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |
21,10 → 21,10 |
// arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad |
int arctan_i(long signed int x, long signed int y) |
int arctan_i( signed int x, signed int y) |
{ |
short int change_xy = 0; |
long signed int i; |
signed int i; |
long signed int wert; |
int return_value; |
44,15 → 44,15 |
if (wert <=268) //0...0.0,268 entsprechend 0..15 Grad |
{ |
return_value = (int)((wert*100)/(268-0)*(15-0)/100) +0; |
return_value = (signed int)((wert*100)/(268-0)*(15-0)/100) +0; |
} |
else if (wert <=578) //0,268...0.0,568 entsprechend 15..30 Grad |
{ |
return_value = (int)((((wert-268)*100)/(578-268)*(30-15))/100) +15; |
return_value = (signed int)((((wert-268)*100)/(578-268)*(30-15))/100) +15; |
} |
else //0,568...1 entsprechend 30..45 Grad |
{ |
return_value = (int)((((wert-578)*50)/(1000-578)*(45-30))/50) +30; |
return_value = (signed int)((((wert-578)*50)/(1000-578)*(45-30))/50) +30; |
} |
if (change_xy == 0) return_value = 90-return_value; //Quadrant 45..90 Grad |
/branches/salvo_gps/math.h |
---|
7,4 → 7,4 |
extern signed int sin_i(signed int winkel); |
extern signed int cos_i(signed int winkel); |
extern signed int arctan_i(long signed int x, long signed int y); |
extern signed int arctan_i(signed int x, signed int y); |