/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 |