Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 224 → Rev 225

/branches/salvo_gps/GPS.c
14,7 → 14,7
Auswertung der Daten vom GPS im ublox Format
Hold Modus mit PID Regler
Komm heim zu Papi Funktion
Stand 5.10.2007
Stand 6.10.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#include "main.h"
45,7 → 45,7
short int ublox_msg_state = UBLOX_IDLE;
static uint8_t chk_a =0; //Checksum
static uint8_t chk_b =0;
short int gps_state,gps_sub_state;
short int gps_state,gps_sub_state; //Zustaende der Statemachine
short int gps_updte_flag;
signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
56,12 → 56,11
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 uint8_t *ptr_payload_data;
static uint8_t *ptr_pac_status;
long int dist_flown;
 
long int dist_flown,dist_frm_start_east,dist_frm_start_north;
 
short int Get_GPS_data(void);
 
NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
294,7 → 293,7
static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert
signed int n,diff_v;
long signed int dev,n_l;
 
signed int dist_frm_start_east,dist_frm_start_north;
switch (cmd)
{
 
307,6 → 306,7
// Erst mal initialisieren
cnt = 0;
gps_tick = 0;
hold_fast = 0;
int_east = 0, int_north = 0;
gps_reg_x = 0, gps_reg_y = 0;
dist_east = 0, dist_north = 0;
354,6 → 354,7
// aktuelle positionsdaten abspeichern
if (gps_rel_act_position.status > 0)
{
hold_fast = 0;
int_east = 0, int_north = 0;
gps_reg_x = 0, gps_reg_y = 0;
dist_east = 0, dist_north = 0;
406,41 → 407,45
{
gps_tick++;
int d1,d2,d3;
d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel
d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
if (d3 > GPS_G2T_DIST_MAX_STOP)
{
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
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_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
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 //Schon ziemlich nahe am Ziel, deswegen abbremsen
{
hold_fast = 0; // Wieder normal regeln
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
{
if (d3 > 0)
if (d3 > GPS_G2T_DIST_HOLD)
{
dist_flown += GPS_G2T_V_RAMP_DWN; // 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
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
}
else //Ziel erreicht, Regelung beenden
else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
{
gps_rel_hold_position.utm_east = 0;
gps_rel_hold_position.utm_north = 0;
gps_sub_state = GPS_HOME_FINISHED;
if (gps_rel_hold_position.utm_east > 1) gps_rel_hold_position.utm_east--;
else if (gps_rel_hold_position.utm_east <-1 ) gps_rel_hold_position.utm_east++;
if (gps_rel_hold_position.utm_north > 1) gps_rel_hold_position.utm_north--;
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;
}
}
}
}
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
return (GPS_STST_OK);
478,6 → 483,12
int_east -= dist_east;
int_north -= dist_north;
}
if (hold_fast > 0) //Im Schnellen Mode Integrator abschalten
{
int_east = 0;
int_north = 0;
}
 
// Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
// 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
489,6 → 500,7
 
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
 
//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
/branches/salvo_gps/README_Gps.txt
1,6 → 1,6
*********************************************************************
GPS Implementierung von Peter Muehlenbrock ("Salvo") fürMikrokopter/FlightCrtl
Stand 3.10.2007
Stand 6.10.2007
Verwendung der SW ohne Gewaehr. Siehe auch die Lizenzbedingungen in den Files gps.c, math.c
*********************************************************************
 
49,11 → 49,20
der Magnetkompass in die waagrechte Lage kommt.
Ein 3D Kompass ist damit überflüssig.
 
GPS Komm heim zu Papi Funktion
Voraussetzungen wie bei GPS Hold.
Die Funktion kann im Flug aktivert werden durch Setzen des Hoehenreglerschalters.
Holgers Code habe ich so abgeändert daß der Höhenregler mit "Parameter_MaxHoehe" ab 50 aktiviert wird.
Mit einem Schalter mit neutrallage können damit die Funktionen
Alles aus - Hoehenregler ein, GPS Home Aus - Hoehenregler Ein UND GPS Home ein
aktiviert werden. Ist noch nicht die beste Lösung und werde ich noch anpassen
 
 
Bekannte Schwächen:
Leichte Schwingneigung ca. +-3m, bei längerer Neigung weicht der Ersatzkompass ab.
PID Regler muss noch besser parametriert werden.
 
 
 
 
 
/branches/salvo_gps/fc.c
857,10 → 857,11
*/
/*
DebugOut.Analog[7] = GasMischanteil;
*/
DebugOut.Analog[8] = KompassValue;
*/
 
DebugOut.Analog[7] = dist_flown;
DebugOut.Analog[8] = dist_frm_start_north;
// DebugOut.Analog[8] = dist_frm_start_north;
 
/* DebugOut.Analog[0] = GPS_hdng_rel_2trgt;
DebugOut.Analog[1] = GPS_dist_2trgt;
/branches/salvo_gps/gps.h
1,7 → 1,7
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Peter Muehlenbrock ("Salvo")
// Definitionen fuer Modul GPS
// Stand 4.10.007
// Stand 6.10.007
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
extern signed int GPS_Nick;
extern signed int GPS_Roll;
81,7 → 81,7
 
//nur provisorisch fuer Debugausgaben
extern signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
extern long int dist_flown,dist_frm_start_east,dist_frm_start_north;
extern long int dist_flown;
 
// Zustaende der zentralen GPS statemachine
#define GPS_CRTL_IDLE 0 //
101,16 → 101,15
#define GPS_STST_ERR 2 // Fehler
 
// GPS Lageregler
#define GPS_NICKROLL_MAX 30 // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll
#define GPS_DIST_MAX 300 // Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm)
#define GPS_NICKROLL_MAX 35 // 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
 
// GPS G2T /Go to Target Regler
//#define GPS_G2T_DIST_MAX_START 50 // Bei dieser Entfernung vom Startpunkt soll die Geschwindigkeit den maximal Wert erreichen (in 10 cm)
#define GPS_G2T_DIST_MAX_STOP 80 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm)
//#define GPS_G2T_DIST_HOLD 30 // Bei dieser Entfernung vom Zielpunkt soll auf Normale Lageregelung umgeschaltet werden( in 10 cm)
#define GPS_G2T_V_MAX 4 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird.
#define GPS_G2T_V_RAMP_DWN 1 // Gschwindigkeit (in 10cm/0.25ekunden) in der Nahe der Home Position um abzubremsen
#define GPS_G2T_TOL 30 // Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht
#define GPS_G2T_DIST_MAX_STOP 70 // 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_TOL 60 // Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht