Rev 662 |
Blame |
Last modification |
View Log
| RSS feed
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Peter Muehlenbrock alias Salvo
// Definitionen fuer Modul GPS
// Stand 9.1.2008
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
extern signed int GPS_Nick;
extern signed int GPS_Roll;
extern void GPS_Neutral(void);
extern void Get_Ublox_Msg(uint8_t rx) ;
extern short int Get_GPS_data(void);
extern short int Get_Rel_Position(void);
extern void GPS_Save_Home(void);
extern short int GPS_CRTL(short int cmd);
typedef struct {
unsigned long utm_itow; // time of week
long utm_east; // UTM Ost in cm
long utm_north; // UTM Nord in cm
long utm_alt; // hoehe in cm
uint8_t utm_zone; //
uint8_t utm_hem; // Hemisphere Indicator
uint8_t status; // 0: kein gueltiges Paket 1: alles ok
} NAV_POSUTM_t;
typedef struct {
unsigned long itow; // time of week
uint8_t gpsfix_type;// 3=3D Fix
uint8_t nav_status_flag;
uint8_t nav_diff_status;
uint8_t nav_resevd;
long nav_tff; // Time to First Fix in ms
long nav_msss; // ms since startup
uint8_t status; // 0: kein gueltiges Paket 1: alles ok
} NAV_STATUS_t;
typedef struct {
unsigned long itow;
long speed_n; // in cm/s
long speed_e; // in cm/s
long speed_alt; // in cm/s
unsigned long speed_3d; // in cm/s
unsigned long speed_gnd; // V ueber Grund in cm/s
long heading; // Richtung in deg/10000
unsigned long sacc; // Speed Genauigkeit in cm/s
unsigned long cacc; // Richtungsgenauigkeit in deg
uint8_t status; // 0: kein gueltiges Paket 1: alles ok
} NAV_VELNED_t;
typedef struct {
long utm_east; // UTM Ost in 10 cm
long utm_north; // UTM Nord in 10 cm
long utm_alt; // hoehe in 10 cm
unsigned long speed_gnd; // V ueber Grund in 10cm/s
unsigned heading; // Richtung in Grad
uint8_t status; // 0: keine gueltigen Daten 1: alles ok
} GPS_ABS_POSITION_t;
typedef struct { // Struktur fuer Relative GPS Daten (bezogen z.B. auf Home Position)
int utm_east; // UTM Ost in 10 cm
int utm_north; // UTM Nord in 10 cm
int utm_alt ; // UTM Altitude in 10 cm
uint8_t status; // 0: keine gueltigen Daten 1: alles ok
} GPS_REL_POSITION_t;
extern GPS_ABS_POSITION_t gps_act_position;
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 short int gps_state,gps_sub_state;
extern unsigned int gps_alive_cnt;
// Zustaende der zentralen GPS statemachine
#define GPS_CRTL_IDLE 0 // Inaktiv
#define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv
#define GPS_CRTL_HOME_ACTIVE 2 // Rueckflug zur Basis Aktiv
#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_STOP 0 // Lageregelung soll deaktiviert werden
#define GPS_CMD_REQ_INIT 1 // Initialisierung
#define GPS_CMD_REQ_HOLD 3 // Lageregelung soll aktiviert werden
#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
// GPS Lageregler
#define GPS_USR_PAR_FKT 4 //Faktor durch den die Userparameter geteilt werden
#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 20 // Wenn der Nick oder Roll Stickwerte groesser sind, wird GPS_HOLD deaktiviert
#define GPS_V 8 // Teilerfaktor Regelabweichung zu Ausgabewert
// Konstanten fuer Verstaerkung fuer Speed Werte in Abhaengigkeit vom SpeedWert (cm/sek)
// um eine exponentielle Verstaerkung zu erreichen
#define DIFF_Y_N_MAX 1 // Verstaerkung bei Eingangswert = DIFF_X_N_MAX im Normal (Hold) Mode
//#define DIFF_X_N_MAX 200 // bei diesem Eingangswert ist die Verstaerkung = DIFF_Y_N_MAX
#define DIFF_Y_F_MAX 1 // Verstaerkung bei Eingangswert = DIFF_X_F_MAX im Fast (Coming Home) Mode
//#define DIFF_X_F_MAX 500 // bei diesem Eingangswert ist die Verstaerkung = DIFF_Y_F_MAX
// P-Regler Verstaerkung
#define GPS_PROP_NRML_V 2 //maximale Verstaerkung im Normalen Holdmode
#define GPS_PROP_FAST_V 4 //maximale Verstaerkung im Fast mode
// GPS G2T /Go to Target Regler
#define GPS_G2T_DIST_MAX_STOP 100 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm)
#define GPS_G2T_DIST_HOLD 50 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler Geschwindigkeit eingeregelt
#define GPS_G2T_FAST_TOL 200 // 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 100 // 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_V_MAX 15 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird.
#define GPS_G2T_V_RAMP_DWN 8 // Geschwindigkeit (in 10cm/0.25ekunden) in der Naehe der Home Position um abzubremsen
#define GPS_G2T_V_MIN 3 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition.