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