Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 235 → Rev 236

/branches/salvo_gps/GPS.c
13,7 → 13,7
Peter Muehlenbrock
Auswertung der Daten vom GPS im ublox Format
Hold Modus mit PID Regler
Komm heim zu Papi Funktion
Rückstuerz zur Basis Funktion
Stand 6.10.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
56,7 → 56,7
unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt
static short int hold_fast; //Flag fuer Hold Regler
static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
static uint8_t *ptr_payload_data;
static uint8_t *ptr_pac_status;
long int dist_flown;
286,12 → 286,13
//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 signed int int_east,int_north; //Integrierer
static signed int dist_north,dist_east;
static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert)
static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert
signed int n,diff_v;
static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
long signed int dev,n_l;
signed int dist_frm_start_east,dist_frm_start_north;
switch (cmd)
307,6 → 308,7
cnt = 0;
gps_tick = 0;
hold_fast = 0;
hold_reset_int = 0; // Integrator enablen
int_east = 0, int_north = 0;
gps_reg_x = 0, gps_reg_y = 0;
dist_east = 0, dist_north = 0;
313,6 → 315,7
diff_east_f = 0, diff_north_f= 0;
diff_east = 0, diff_north = 0;
dist_flown = 0;
gps_g2t_act_v = 0;
gps_sub_state = GPS_CRTL_IDLE;
// aktuelle positionsdaten abspeichern
if (gps_rel_act_position.status > 0)
354,7 → 357,8
// aktuelle positionsdaten abspeichern
if (gps_rel_act_position.status > 0)
{
hold_fast = 0;
hold_fast = 0;
hold_reset_int = 0; // Integrator enablen
int_east = 0, int_north = 0;
gps_reg_x = 0, gps_reg_y = 0;
dist_east = 0, dist_north = 0;
416,22 → 420,28
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
{
hold_fast = 1; // Schnell Regeln
dist_flown += (GPS_G2T_V_MAX); // Vorgabe der Strecke anhand der Geschwindigkeit
if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
}
else hold_fast = 0; // Wieder normal regeln
else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
{
if (gps_g2t_act_v > 0) gps_g2t_act_v--;
if (gps_g2t_act_v <= (GPS_G2T_V_MAX/2)) hold_fast = 0; // Wieder normal regeln
}
}
else //Schon ziemlich nahe am Ziel, deswegen abbremsen
{
hold_fast = 0; // Wieder normal regeln
hold_fast = 0; // Wieder normal regeln
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
{
if (d3 > GPS_G2T_DIST_HOLD)
{
dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
if (gps_g2t_act_v < GPS_G2T_V_RAMP_DWN) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000;
dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000;
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
445,7 → 455,11
else if (gps_rel_hold_position.utm_north < -1 ) gps_rel_hold_position.utm_north++;
if ((abs(gps_rel_hold_position.utm_east) <= 1) && (abs(gps_rel_hold_position.utm_north) <=1)) gps_sub_state = GPS_HOME_FINISHED;
}
}
}
else
{
if (gps_g2t_act_v > 0) gps_g2t_act_v--;
}
}
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
return (GPS_STST_OK);
460,7 → 474,7
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 >0) // nur wenn neue GPS Daten vorliegen
{
gps_updte_flag = 0;
477,13 → 491,13
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 1.10.2007 Begrenzung
#define GPSINT_MAX 30000 //neuer Wert ab 7.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;
int_north -= dist_north;
}
if (hold_fast > 0) //Im Schnellen Mode Integrator abschalten
if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten
{
int_east = 0;
int_north = 0;
493,7 → 507,7
// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
#define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
#define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
signed long dist;
signed long dist,int_east1,int_north1;
int phi;
phi = arctan_i(abs(dist_north),abs(dist_east));
dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
500,11 → 514,20
 
diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
if (hold_fast > 0) diff_v = 10; // im schnellen Modus keine staerkere Wirkung des Differenzierers
if (hold_fast > 0) diff_v = 10; //im schnellen Modus schwache Wirkung des Differenzierers
 
//I Werte begrenzen
#define INT1_MAX (20 * GPS_V)
int_east1 = ((((long)int_east) * Parameter_UserParam2)/32);
int_north = ((((long)int_north) * Parameter_UserParam2)/32);
if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen
else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX;
if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen
else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX;
 
//PID Regler Werte aufsummieren
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse
gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil Y Achse
gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse
gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse
 
//Ziel-Richtung bezogen auf Nordpol bestimmen
GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
536,7 → 559,6
GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
#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 6.10.2007
Stand 7.10.2007
Verwendung der SW ohne Gewaehr. Siehe auch die Lizenzbedingungen in den Files gps.c, math.c
*********************************************************************
 
7,7 → 7,7
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.
mussen mit 4 HZ Updaterate aktiviert sein. Anschluss an RX Port der FlightCRtl. Baudrate ist 57600
 
Software-Voraussetzungen:
in fc.h müsen die Neutralwerte ACC_X_NEUTRAL und ACC_Y_NEUTRAL
/branches/salvo_gps/fc.c
52,11 → 52,11
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 4.109.2007
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 6.10.2007
/*
Driftkompensation fuer Gyros verbessert
Linearsensor mit fixem neutralwert
Ersatzkompass abgeleitet aus magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion
Linearsensor mit fixem Neutralwert
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
*/
 
#include "main.h"
661,7 → 661,7
if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR) AdNeutralRoll++;
if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR) AdNeutralRoll--;
}
else // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
/* else // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
{
if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR) AdNeutralNick++;
if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR) AdNeutralNick--;
668,7 → 668,8
if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR) AdNeutralRoll++;
if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR) AdNeutralRoll--;
}
}
*/ }
 
// Salvo End
 
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
770,31 → 771,26
}
// Salvo End *************************
 
// Salvo 4.10.2007
// Salvo 6.10.2007
// GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
//GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
if ((Parameter_MaxHoehe > 200) && ((abs(StickRoll)) < GPS_STICK_HOLDOFF)
&& ( (abs(StickNick)) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
{
if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd = GPS_CMD_REQ_HOME;
n = GPS_CRTL(gps_cmd);
}
else
if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(StickRoll) < GPS_STICK_HOLDOFF) && (abs(StickNick) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
{
if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && ((abs(StickRoll)) < GPS_STICK_HOLDOFF)
&& ( (abs(StickNick)) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
{
if (Parameter_MaxHoehe > 200)
{
if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd = GPS_CMD_REQ_HOME;
n = GPS_CRTL(gps_cmd);
}
else
{
if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd = GPS_CMD_REQ_HOLD;
n= GPS_CRTL(gps_cmd);
}
}
else (n= GPS_CRTL(GPS_CMD_STOP)); //GPS Lageregelung beenden
 
if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd = GPS_CMD_REQ_HOLD;
n= GPS_CRTL(gps_cmd);
}
else
{
n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/branches/salvo_gps/fc.h
10,7 → 10,6
#define ACC_NEUTRAL_FIXED 1 // wenn eins werden die Neutralwerte fuer den ACC Sensor festeingestellt
#define ACC_X_NEUTRAL 518 // ADC Wandler Wert in Neutrallage (0g): Vom individuellen Sensor abhaengig
#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
// Salvo End
/branches/salvo_gps/gps.h
1,7 → 1,7
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Peter Muehlenbrock ("Salvo")
// Definitionen fuer Modul GPS
// Stand 6.10.007
// Stand 7.10.007
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
extern signed int GPS_Nick;
extern signed int GPS_Roll;
101,15 → 101,17
#define GPS_STST_ERR 2 // Fehler
 
// GPS Lageregler
#define GPS_NICKROLL_MAX 35 // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll
#define GPS_NICKROLL_MAX 40 // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll
#define GPS_DIST_MAX 400 // Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm)
#define GPS_STICK_HOLDOFF 25 // Wenn der Nick oder Roll Stickwerte groesser sind, wird GPS_HOLD deaktiviert
#define GPS_V 8 // Teilerfaktor intern --> Ausgabewert
 
// GPS G2T /Go to Target Regler
#define GPS_G2T_DIST_MAX_STOP 70 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm)
#define GPS_G2T_DIST_MAX_STOP 60 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm)
#define GPS_G2T_DIST_HOLD 30 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler geschwindigkeit eingeregelt
#define GPS_G2T_V_MAX 6 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird.
#define GPS_G2T_V_RAMP_DWN 2 // Geschwindigkeit (in 10cm/0.25ekunden) in der Nahe der Home Position um abzubremsen
#define GPS_G2T_V_MAX 12 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird.
#define GPS_G2T_V_RAMP_DWN 6 // Geschwindigkeit (in 10cm/0.25ekunden) in der Nahe der Home Position um abzubremsen
#define GPS_G2T_V_MIN 3 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition.
#define GPS_G2T_TOL 60 // Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht