Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 208 → Rev 209

/branches/salvo_gps/GPS.c
39,22 → 39,20
#define UBLOX_SYNCH1_CHAR 0xB5
#define UBLOX_SYNCH2_CHAR 0x62
 
signed int GPS_Nick = 0;
signed int GPS_Roll = 0;
short int ublox_msg_state = UBLOX_IDLE;
static uint8_t chk_a =0; //Checksum
static uint8_t chk_b =0;
short int gps_state;
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
signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
signed int ;
signed int GPS_Nick = 0;
signed int GPS_Roll = 0;
short int ublox_msg_state = UBLOX_IDLE;
static uint8_t chk_a =0; //Checksum
static uint8_t chk_b =0;
short int gps_state;
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
signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
static unsigned int rx_len;
unsigned int cnt0,cnt1,cnt2; //******Provisorisch
static unsigned int ptr_payload_data_end;
unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
 
static uint8_t *ptr_payload_data;
static uint8_t *ptr_pac_status;
87,7 → 85,6
gps_int_x = 0;
gps_int_y = 0;
gps_alive_cnt = 0;
 
}
 
// Home Position sichern falls Daten verfuegbar sind.
138,7 → 135,6
if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
{
cnt1++; //**** noch Rausschmeissen
if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
{
gps_act_position.utm_east = actual_pos.utm_east/10;
170,7 → 166,6
*/
void Get_Ublox_Msg(uint8_t rx)
{
 
switch (ublox_msg_state)
{
 
384,13 → 379,13
int_east -= dist_east;
int_north -= dist_north;
}
//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 1
// 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
#define GPS_DIFF_MAX_D 40 //Entfernung bei der maximale Verstaerkung erreicht wird in (dm)
#define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
signed long dist;
int phi;
phi = arctan_i(abs(dist_north),abs(dist_east));
phi = arctan_i(abs(dist_north),abs(dist_east));
if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln
{
dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen
402,7 → 397,7
dist = abs((dist *1000) / (long) cos_i(phi));
}
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;
if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
 
//PID Regler Werte aufsummieren
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east_f * 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 1.10.2007
Stand 3.10.2007
Verwendung der SW ohne Gewaehr. Siehe auch die Lizenzbedingungen in den Files gps.c, math.c
*********************************************************************
 
26,8 → 26,9
Parametrierung:
Der GPS Hold Regler ist ein PID Regler, der ueber die UserParameter1(P), 2(I) und D(3) gesteuert wird.
UserParameter1 beschreibt den P-Anteil, UserParameter2 den I-Anteil und UserParameter3 den D-Anteil.
Hier kann und muss gespielt werden. Wenn alle 0 sind, ist der Regler deaktiviert.
Standardwerte sind 8 für den P-Anteil, 2 für den I-Anteil und 10 für den D-Anteil.
Hier kann und muss gespielt werden.
Standardwerte sind 8 für den P-Anteil, 3 für den I-Anteil und 12 für den D-Anteil.
Wenn alle 0 sind, ist der Regler deaktiviert.
 
Voraussetzungen für GPS_Hold:
Neben den genannten HW und SW Voraussetzungen muß beim Kalibrieren das GPS MOdul bereits Positionsdaten liefern.
49,7 → 50,7
Ein 3D Kompass ist damit überflüssig.
 
Bekannte Schwächen:
Leichte Schwingneigung ca. +-5m, bei längerer Neigung weicht der Ersatzkompass ab.
Leichte Schwingneigung ca. +-3m, bei längerer Neigung weicht der Ersatzkompass ab.
PID Regler muss noch besser parametriert werden.
 
 
/branches/salvo_gps/fc.c
767,8 → 767,7
}
// Salvo End *************************
 
// Salvo 15.9.2007 GPS Hold Aktiveren mit dem Hoehenschalter aber nur wenn Knueppel in Ruhelage sind
//if ((Parameter_MaxHoehe > 200) && ( (abs(StickRoll)) < GPS_STICK_HOLDOFF) && ( (abs(StickNick)) < GPS_STICK_HOLDOFF))
// Salvo 2.10.2007 GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && ((abs(StickRoll)) < GPS_STICK_HOLDOFF)
&& ( (abs(StickNick)) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
{
820,26 → 819,27
if(!TimerWerteausgabe--)
{
TimerWerteausgabe = 49;
// DebugOut.Analog[0] = MesswertNick;
// DebugOut.Analog[1] = MesswertRoll;
// DebugOut.Analog[2] = MesswertGier;
DebugOut.Analog[0] = MesswertNick;
DebugOut.Analog[1] = MesswertRoll;
// DebugOut.Analog[2] = MesswertGier;
// DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
// DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
// DebugOut.Analog[2] = Mittelwert_AccNick;
// DebugOut.Analog[3] = Mittelwert_AccRoll;
DebugOut.Analog[2] = Mittelwert_AccNick;
DebugOut.Analog[3] = Mittelwert_AccRoll;
DebugOut.Analog[4] = MesswertGier;
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
/*
 
DebugOut.Analog[7] = GasMischanteil;
DebugOut.Analog[8] = KompassValue;
*/
DebugOut.Analog[0] = GPS_hdng_rel_2trgt;
 
/* DebugOut.Analog[0] = GPS_hdng_rel_2trgt;
DebugOut.Analog[1] = GPS_dist_2trgt;
DebugOut.Analog[2] = GPS_Nick;
DebugOut.Analog[3] = GPS_Roll;
DebugOut.Analog[7] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
DebugOut.Analog[8] = KompassValue;
*/
/*
DebugOut.Analog[7] = gps_rel_act_position.utm_east;
DebugOut.Analog[8] = gps_rel_act_position.utm_north;
846,7 → 846,6
*/
DebugOut.Analog[9] = gps_reg_x;
DebugOut.Analog[10] = gps_reg_y;
// DebugOut.Analog[11] = gps_alive_cnt;
DebugOut.Analog[11] = GPS_hdng_abs_2trgt;
 
 
906,11 → 905,7
int tmp_int;
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert
{
// Salvo 15.9.2007 Grenzwert fuer Hoehenschalter kleiner, damit
// mit dem Schalter die Zusatende AUS- Hoehenregler AN -- Hoehenreger und GPS an
//durchlaufen werden koennen
if(Parameter_MaxHoehe < 50)
// Salvo End
{
SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
HoehenReglerAktiv = 0;
/branches/salvo_gps/gps.h
1,7 → 1,7
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Peter Muehlenbrock ("Salvo")
// Definitionen fuer Modul GPS
// Stand 19.9.007
// Stand 2.10.007
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
extern signed int GPS_Nick;
extern signed int GPS_Roll;
71,7 → 71,6
extern GPS_ABS_POSITION_t gps_home_position;
extern GPS_REL_POSITION_t gps_rel_act_position;
extern GPS_REL_POSITION_t gps_rel_hold_position;
extern unsigned int cnt0,cnt1,cnt2;
extern short int gps_state;
extern signed int GPS_hdng_abs_2trgt;
extern signed int GPS_hdng_rel_2trgt;
92,13 → 91,11
#define GPS_CMD_REQ_HOME 4 // Das Heimfliegen soll aktiviert werden
 
// Statusmeldungen der zentralen GPS statemachine
#define GPS_STST_OK 0 // Kommando erfolgreich und abgeschlossen
#define GPS_STST_PEND 1 // Kommando noch nicht komplett durchgefuehrt
#define GPS_STST_ERR 2 // Fehler
#define GPS_STST_OK 0 // Kommando erfolgreich und abgeschlossen
#define GPS_STST_PEND 1 // Kommando noch nicht komplett durchgefuehrt
#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 500 //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.
#define GPS_STICK_HOLDOFF 25 // Wenn der Nick oder Roll groesser ist, wird GPS_HOLD deaktiviert
#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_STICK_HOLDOFF 25 // Wenn der Nick oder Roll Stickwerte groesser sind, wird GPS_HOLD deaktiviert