Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 265 → Rev 266

/branches/salvo_gps/GPS.c
80,6 → 80,7
{
ublox_msg_state = UBLOX_IDLE;
gps_state = GPS_CRTL_IDLE;
gps_sub_state = GPS_CRTL_IDLE;
actual_pos.status = 0;
actual_speed.status = 0;
actual_status.status = 0;
332,7 → 333,7
// in Winkel 0...360 Grad umrechnen
if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
else hdng_2home = (270 - hdng_2home);
dist_2home = get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
dist_2home = (int)get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
gps_state = GPS_CRTL_HOME_ACTIVE;
return (GPS_STST_OK);
}
388,7 → 389,8
GPS_Roll = 0;
gps_int_x = 0;
gps_int_y = 0;
gps_state = GPS_CRTL_IDLE;
gps_sub_state = GPS_CRTL_IDLE;
gps_state = GPS_CRTL_IDLE;
return (GPS_STST_OK);
break;
 
418,43 → 420,43
if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
{
// hold_fast = 1; // Schnell Regeln
hold_reset_int = 1; // Integrator ausschalten
if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
{
if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
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
gps_sub_state = GPS_HOME_FAST_IN_TOL;
}
else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
{
if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
gps_sub_state = GPS_HOME_FAST_OUTOF_TOL;
}
hold_fast = 1; // Regler fuer schnellen Flug
hold_reset_int = 1; // Integrator ausschalten
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 if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen
{
hold_reset_int = 0; // Integrator einschalten
if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL))
{
if (gps_g2t_act_v > GPS_G2T_V_RAMP_DWN) gps_g2t_act_v = GPS_G2T_V_RAMP_DWN; // Geschwindigkeit zu hoch
else 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
gps_sub_state = GPS_HOME_RMPDWN_IN_TOL;
dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
gps_sub_state = GPS_HOME_RMPDWN_IN_TOL;
}
else
{
if (gps_g2t_act_v > 1) gps_g2t_act_v--;
dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL;
}
hold_fast = 0; // Wieder normal regeln
hold_reset_int = 1; // Integrator ausschalten
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 //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
{
474,6 → 476,7
gps_sub_state = GPS_HOME_FINISHED;
}
}
else gps_sub_state = GPS_HOME_OUTOF_TOL;
}
}
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
520,8 → 523,8
// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
signed long dist,int_east1,int_north1;
int phi;
phi = arctan_i(abs(dist_north),abs(dist_east));
dist = (long) (get_dist(dist_east,dist_north,phi)); //Zunaechst Entfernung zum Ziel ermitteln
phi = arctan_i(abs(dist_north),abs(dist_east));
dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
 
if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer
{
534,6 → 537,10
if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen
}
 
int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung
if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
else diff_p = GPS_PROP_NRML_V;
 
//I Werte begrenzen
#define INT1_MAX (20 * GPS_V)
int_east1 = ((((long)int_east) * Parameter_UserParam2)/32);
543,13 → 550,9
if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen
else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX;
 
int diff_p;
if (hold_fast == 0) diff_p = 1;
else diff_p = 2; //im schnellen Modus Proportionalanteil staerken
 
//PID Regler Werte aufsummieren
gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse
gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse
gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse
gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((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);
/branches/salvo_gps/fc.c
933,7 → 933,10
{
if(Parameter_MaxHoehe < 50)
{
SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
//Salvo 10.10.2007 Um Absacken beim Einschalten zu verhindern
// SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
SollHoehe = HoehenWert -10; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
// Salvo End
HoehenReglerAktiv = 0;
}
else
/branches/salvo_gps/gps.h
87,13 → 87,13
#define GPS_CRTL_IDLE 0 //
#define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv
#define GPS_CRTL_HOME_ACTIVE 2 // Rueckflug zur Basis Aktiv
#define GPS_HOME_FINISHED 3 // Rueckflug zur Basis abgeschlossen
#define GPS_HOME_FAST_IN_TOL 4 // Rueckflug: Aktuelle Position innerhalb der Toleranz
#define GPS_HOME_FAST_OUTOF_TOL 5 // Rueckflug: Aktuelle Position ausserhalb der Toleranz
#define GPS_HOME_RMPDWN_IN_TOL 6 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz
#define GPS_HOME_RMPDWN_OUTOF_TOL 7 // Rueckflug: beim Abbremsen Position ausserhalb der Toleranz
#define GPS_HOME_IN_TOL 8 // Rueckflug: Nahe am Ziel innerhalb der Toleranz
#define GPS_HOME_OUTOF_TOL 9 // Rueckflug: Nahe am Ziel ausserhalb der Toleranz
#define GPS_HOME_FAST_IN_TOL 3 // Rueckflug: Aktuelle Position innerhalb der Toleranz
#define GPS_HOME_FAST_OUTOF_TOL 4 // Rueckflug: Aktuelle Position ausserhalb der Toleranz
#define GPS_HOME_RMPDWN_IN_TOL 5 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz
#define GPS_HOME_RMPDWN_OUTOF_TOL 6 // Rueckflug: beim Abbremsen Position ausserhalb der Toleranz
#define GPS_HOME_IN_TOL 7 // Rueckflug: Nahe am Ziel innerhalb der Toleranz
#define GPS_HOME_OUTOF_TOL 8 // Rueckflug: Nahe am Ziel ausserhalb der Toleranz
#define GPS_HOME_FINISHED 9 // Rueckflug zur Basis abgeschlossen
 
// Kommandokonstanten fuer die zentrale GPS Statemachine
#define GPS_CMD_REQ_INIT 0 // Initialisierung
111,20 → 111,22
#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 Regelabweichung zu Ausgabewert
// Differenzierer Kennwerte fuer von Distanz abhaengige Verstaerkung.
// Differenzierer Kennwerte fuer von Distanz abhaengige Verstaerkung, abhaengig vom Modus.
#define GPS_DIFF_NRML_MAX_V 20 //maximale Verstaerkung * 10
#define GPS_DIFF_NRML_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
#define GPS_DIFF_FAST_MAX_V 20 //maximale Verstaerkung * 10 im Fast mode
#define GPS_DIFF_FAST_MAX_D 40 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm im Fast mode
#define GPS_DIFF_FAST_MAX_V 10 //maximale Verstaerkung * 10 im Fast mode
#define GPS_DIFF_FAST_MAX_D 80 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm im Fast mode
// P-Regler Verstaerkung
#define GPS_PROP_NRML_V 2 //maximale Verstaerkung * 2
#define GPS_PROP_FAST_V 2 //maximale Verstaerkung * 2
 
 
// GPS G2T /Go to Target Regler
#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 10 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird.
#define GPS_G2T_V_RAMP_DWN 5 // Geschwindigkeit (in 10cm/0.25ekunden) in der Nahe der Home Position um abzubremsen
#define GPS_G2T_V_MAX 8 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird.
#define GPS_G2T_V_RAMP_DWN 4 // Geschwindigkeit (in 10cm/0.25ekunden) in der Nahe der Home Position um abzubremsen
#define GPS_G2T_V_MIN 2 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition.
#define GPS_G2T_FAST_TOL 80 // Bei grosser Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht
#define GPS_G2T_NRML_TOL 40 // Bei kleiner Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht
#define GPS_G2T_NRML_TOL 50 // Bei kleiner Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht
 
 
/branches/salvo_gps/math.c
42,7 → 42,7
 
// Wert durch lineare Interpolation ermitteln
if ((y == 0) && (x == 0)) wert =1; // Division durch 0 nicht erlaubt
else wert= abs(((long) x*1000)/((long)y));
else wert= abs(((long)x*1000)/((long)y));
 
if (wert <=268) //0...0.0,268 entsprechend 0..15 Grad
{
108,7 → 108,7
}
 
// Aus x,y und Winkel Distanz ermitteln
int get_dist(signed int x, signed int y, signed int phi)
long get_dist(signed int x, signed int y, signed int phi)
{
long dist;
if (abs(x) > abs(y) )
121,6 → 121,6
dist = (long) y;
dist = abs((dist *1000) / (long) cos_i(phi));
}
return (int)dist;
return dist;
}
 
/branches/salvo_gps/math.h
8,4 → 8,4
extern signed int sin_i(signed int winkel);
extern signed int cos_i(signed int winkel);
extern signed int arctan_i(signed int x, signed int y);
extern int get_dist(signed int x, signed int y, signed int phi);
extern long get_dist(signed int x, signed int y, signed int phi);