/branches/salvo_gps/GPS.c |
---|
12,8 → 12,8 |
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Peter Muehlenbrock |
Auswertung der Daten vom GPS im ublox Format |
Regelung fuer GPS noch nicht implementiert |
Stand 16.9.2007 |
Hold Modus |
Stand 20.9.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |
345,24 → 345,32 |
signed int dist_north,dist_east; |
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; |
#define DIST_MAX 100 //neu ab 19.9. 1900 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_int_x = gps_int_x + (dist_east * Parameter_UserParam1)/16; // Integrator |
gps_int_y = gps_int_y + (dist_north * Parameter_UserParam1)/16; |
if ((gps_int_x >= 4096) || (gps_int_y >= 4096) || (gps_int_x < - 4096) || (gps_int_y <= -4096)) |
gps_int_x = gps_int_x + (dist_east * Parameter_UserParam1)/64; // Integrator |
gps_int_y = gps_int_y + (dist_north * Parameter_UserParam1)/64; |
#define GPSINT_MAX 256 // ************Kleiner machen |
if ((gps_int_x >= GPSINT_MAX) || (gps_int_y >= GPSINT_MAX) || (gps_int_x < -GPSINT_MAX) || (gps_int_y <= -GPSINT_MAX)) |
{ |
gps_int_x -= (dist_east * Parameter_UserParam1)/16; // Integrator stoppen |
gps_int_y -= (dist_north * Parameter_UserParam1)/16; |
gps_int_x -= (dist_east * Parameter_UserParam1)/64; // Integrator stoppen |
gps_int_y -= (dist_north * Parameter_UserParam1)/64; |
} |
gps_reg_x = gps_int_x + (dist_east * Parameter_UserParam2)/16; // P Anteil dazu |
gps_reg_y = gps_int_y + (dist_north * Parameter_UserParam2)/16; |
gps_reg_x = gps_int_x + (dist_east * Parameter_UserParam2)/8; // P Anteil dazu |
gps_reg_y = gps_int_y + (dist_north * Parameter_UserParam2)/8; |
//Richtung bezogen auf Nordpol bestimmen |
GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
//in Winkel 0..360 grad umrechnen |
if ((gps_reg_x >= 0) && (gps_reg_y < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
else if ((gps_reg_x < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
// 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; |
375,25 → 383,25 |
if (abs(gps_reg_x) > abs(gps_reg_y) ) |
{ |
dist = (long)gps_reg_x; //Groesseren Wert wegen besserer genauigkeit nehmen |
dist = abs((dist *1000) / (long) cos_i(GPS_hdng_rel_2trgt)); |
dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
} |
else |
{ |
dist = (long)gps_reg_y; |
dist = abs((dist *1000) / (long) sin_i(GPS_hdng_rel_2trgt)); |
dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
} |
if (dist > 30000) dist = 30000; |
if (dist > 200) dist = 200; |
GPS_dist_2trgt = (int )dist; |
// Winkel und Distanz in Nick und Roll groessen umrechnen |
long int a,b; |
GPS_Roll = (int) -((dist * sin_i(GPS_hdng_rel_2trgt))/(1000*4)); |
GPS_Roll = (int) +((dist * sin_i(GPS_hdng_rel_2trgt))/(1000*4)); |
GPS_Nick = (int) -((dist * cos_i(GPS_hdng_rel_2trgt))/(1000*4)); |
if (GPS_Roll > GPS_ROLL_MAX) GPS_Roll = GPS_ROLL_MAX; //Auf Maxwerte begrenzen |
else if (GPS_Roll < -GPS_ROLL_MAX) GPS_Roll = - GPS_ROLL_MAX; |
if (GPS_Nick > GPS_NICK_MAX) GPS_Nick = GPS_NICK_MAX; |
else if (GPS_Nick < -GPS_NICK_MAX) GPS_Nick = - GPS_NICK_MAX; |
if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
return (GPS_STST_OK); |
} |
/branches/salvo_gps/README_Gps.txt |
---|
0,0 → 1,65 |
********************************************************************* |
GPS Implementierung von Peter Muehlenbrock ("Salvo") fuer Mikrokopter/FlightCrtl |
Stand 19.9.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 |
Software-Voraussetzungen: |
in fc.h muessen die Neutralwerte ACC_X_NEUTRAL, ACC_Y_NEUTRAL und ACC_Z_NEUTRAL |
eingetragen werden. Hierzu den Kopter waagrecht ausrichten (Wasserwaage) und mit dem Mikrokoptertool |
die Neutralwerte ermitteln. |
In timer0.h muess die Ausrichtung des Kompasses bezogen auf die Nordachse des Kopters |
eingetragen sein. KOMPASS_OFFSET legt diesen Wert fest. |
Hexfiles: |
Ich habe bewußt keinen Hexfile generiert, da die Neutralwerte spezifisch je Kopter sind. |
Die SW muß also individuell compiliert werden und der Hexfile in die FlightCrtl eingebaut werden. |
Der Lohn fuer die Muehe ist eine sehr driftarme Fluglageregelung sowie eine von der Startausrichtung |
unabhaengige waagrechte Lage in der Luft. |
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. |
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. |
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 |
fuer ca. 500msec in Neutrallage sind. Weicht einer der Sticks davon ab (Parameter GPS_STICK_HOLDOFF) wird GPS_Hold sofort deaktiviert. |
Damit kann jederzeit die manuelle Kontrolle wieder übernommen werden. |
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. |
Ein 3D Kompass ist damit überfluessig. |
Bekannte Maengel: |
????? |
Ausblick: |
Als nächstes ist die GPS Home Funktion geplant. |
/branches/salvo_gps/fc.c |
---|
95,7 → 95,7 |
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
unsigned char MotorWert[5]; |
volatile unsigned char SenderOkay = 0; |
int StickNick = 0,StickRoll = 0,StickGier = 0; |
int Nick = 0,StickRoll = 0,StickGier = 0; |
char MotorenEin = 0; |
int HoehenWert = 0; |
int SollHoehe = 0; |
/branches/salvo_gps/fc.h |
---|
12,7 → 12,7 |
#define ACC_Y_NEUTRAL 516 // ADC Wandler wert in Neutrallage (0g) |
#define ACC_Z_NEUTRAL 745 // ADC Wandler Wert in Neutrallage (1g) |
#define ACC_WAAGRECHT_LIMIT 100 // Nick und Roll kleiner als dieser Wert gelten als kriterium fuer waagrechte Lage |
#define ACC_WAAGRECHT_LIMIT 100 // Nick und Roll kleiner als dieser Wert gelten als Kriterium fuer waagrechte Lage |
// Salvo End |
//Salvo 2.9.2007 Ersatzkompass: Gyroincrements/Grad als Defaultwert ***** |
// Laut Datenblatt sind die Werte ueber Zeit und Temperatur sehr stabil. |
/branches/salvo_gps/gps.h |
---|
1,7 → 1,7 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Peter Muehlenbrock |
// Peter Muehlenbrock ("Salvo") |
// Definitionen fuer Modul GPS |
// Stand 19.9.007 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
extern signed int GPS_Nick; |
extern signed int GPS_Roll; |
95,5 → 95,5 |
#define GPS_STST_ERR 2 // Fehler |
// GPS Lageregler |
#define GPS_NICK_MAX 10 |
#define GPS_ROLL_MAX 10 |
#define GPS_NICKROLL_MAX 30 //Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
/branches/salvo_gps/math.c |
---|
13,7 → 13,7 |
Peter Muehlenbrock |
Winkelfunktionen sin, cos und arctan in |
brute-force Art: Sehr Schnell, nicht sonderlich genau, aber ausreichend |
Stand 11.9.2007 |
Stand 20.9.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |
53,9 → 53,9 |
return_value = (int)((((wert-578)*50)/(1000-578)*(45-30))/50) +30; |
} |
if (change_xy > 0) return_value = 90-return_value; //Quadrant 45..90 Grad |
if ((x > 0) && (y <0)) return_value = - return_value; |
else if ((x < 0) && (y > 0)) return_value = - return_value; |
if (change_xy == 0) return_value = 90-return_value; //Quadrant 45..90 Grad |
if ((x >= 0) && (y <0)) return_value = - return_value; |
else if ((x < 0) && (y >= 0)) return_value = - return_value; |
return return_value; |
} |