Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 193 → Rev 194

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