Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 185 → Rev 186

/branches/salvo_gps/GPS.c
286,9 → 286,10
static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen
static signed int int_east,int_north; //Integrierer
static signed int dist_north,dist_east;
int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert)
int n;
long int dist;
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;
long signed int dist,d,n_l;
switch (cmd)
{
 
308,12 → 309,16
// aktuelle positionsdaten abespeichern
if (gps_rel_act_position.status > 0)
{
int_east = 0;
int_north = 0;
gps_reg_x = 0;
gps_reg_y = 0;
dist_east = 0;
dist_north = 0;
int_east = 0;
int_north = 0;
gps_reg_x = 0;
gps_reg_y = 0;
dist_east = 0;
dist_north = 0;
diff_east_f = 0;
diff_north_f = 0;
diff_east = 0;
diff_north = 0;
gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
gps_rel_hold_position.status = 1; // gueltige Positionsdaten
353,14 → 358,13
return (GPS_STST_OK);
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 == 1) //nur wenn neue GPS Daten vorliegen
{
gps_updte_flag = 0;
// ab hier wird geregelt
 
diff_east = -dist_east; //Alten Wert schon mal abziehen
diff_north = -dist_north;
diff_east = -dist_east; //Alten Wert fuer Differenzier schon mal abziehen
diff_north = -dist_north;
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;
int_east += dist_east;
368,6 → 372,9
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
 
#define GPSINT_MAX 2048 //neuer Wert ab 25.9.2007 Begrenzung
if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
{
374,15 → 381,31
int_east -= dist_east;
int_north -= dist_north;
}
// P-Anteil Kleine Werte verstaerken, grosse abschwaechen
#define GPS_DIST_P_MAX 200 //maximal 20m
int dist_east_p,dist_north_p;
dist_east_p = dist_east;
dist_north_p = dist_north;
if (dist_east > GPS_DIST_P_MAX) dist_east_p = GPS_DIST_P_MAX; // P-Anteil begrenzen
else if (dist_east < - GPS_DIST_P_MAX) dist_east_p = -GPS_DIST_P_MAX;
if (dist_north > GPS_DIST_P_MAX) dist_north_p = GPS_DIST_P_MAX; // P-Anteil begrenzen
else if (dist_north < - GPS_DIST_P_MAX) dist_north_p = -GPS_DIST_P_MAX;
n = sin_i((dist_east_p*90)/(GPS_DIST_P_MAX));
d = (GPS_DIST_P_MAX * (long)n) /1000;
dist_east_p = (int) d;
n = sin_i((dist_north_p*90)/(GPS_DIST_P_MAX));
d = (GPS_DIST_P_MAX * (long)n) /1000;
dist_north_p = (int) d;
 
//PID Regler
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * Parameter_UserParam3)/2); //
gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse
//PID Regler Werte aufsummieren
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east_p * Parameter_UserParam1)/6)+ ((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)/6)+ ((diff_north_f * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse
 
//Richtung bezogen auf Nordpol bestimmen
//Ziel-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
// in Winkel 0...360 Grad umrechnen
if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
 
404,14 → 427,12
dist = (long)gps_reg_y;
dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
}
// if (dist > 200) dist = 200;
// GPS_dist_2trgt = (int )dist;
 
// Winkel und Distanz in Nick und Rollgroessen umrechnen
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
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);
418,14 → 439,12
else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
 
//Kleine Werte verstaerken, Grosse abschwaechen
long int nick,roll;
int n,r;
r = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));
n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));
n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
GPS_Roll = (int) n_l;
n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V));
roll = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) r)/1000;
nick = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) n)/1000;
GPS_Roll = (int) (roll / GPS_V);
GPS_Nick = (int) (nick / GPS_V);
n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
GPS_Nick = (int) n_l;
if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen
{
432,16 → 451,9
GPS_Roll = 0;
GPS_Nick = 0;
gps_state = GPS_CRTL_IDLE;
return (GPS_STST_ERR);
return (GPS_STST_ERR);
break;
}
/* else // Distanz ok // kann spaeter entfallen, weil eigentlich schon begrenzt
{
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);
}*/
}
else return (GPS_STST_OK);
break;
/branches/salvo_gps/gps.h
96,7 → 96,7
#define GPS_STST_ERR 2 // Fehler
 
// GPS Lageregler
#define GPS_NICKROLL_MAX 30 //Maximaler Einfluss des GPS Lagereglers auf Nick und Roll
#define GPS_NICKROLL_MAX 25 //Maximaler Einfluss des GPS Lagereglers auf Nick und Roll
#define GPS_DIST_MAX 300 //Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm)
//Salvo 16.9.2007 GPS_STICK_HOLDOFF ***************
// Laut Datenblatt sind die Werte ueber Zeit und Temperatur sehr stabil.