WebSVN - FlightCtrl - Blame - Rev 619 - /branches/salvo_gps/Basis

Rev	Author	Line No.	Line
528	salvo	1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		2	// Peter Muehlenbrock ("Salvo")
		3	// Definitionen fuer Modul GPS
		4	// Stand 12.10.007
		5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		6	extern signed int GPS_Nick;
		7	extern signed int GPS_Roll;
542	salvo	8	extern signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
		9	extern signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
602	salvo	10	extern signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
528	salvo	11	extern void GPS_Neutral(void);
		12	extern void Get_Ublox_Msg(uint8_t rx) ;
		13	//extern short int Get_GPS_data(void);
		14	extern short int Get_Rel_Position(void);
		15	extern void GPS_Save_Home(void);
		16	extern short int GPS_CRTL(short int cmd);
		17
		18	typedef struct {
		19	unsigned long utm_itow; // time of week
		20	long utm_east; // UTM Ost in cm
		21	long utm_north; // UTM Nord in cm
		22	long utm_alt; // hoehe in cm
		23	uint8_t utm_zone; //
		24	uint8_t utm_hem; // Hemisphere Indicator
		25	uint8_t status; // 0: kein gueltiges Paket 1: alles ok
		26	} NAV_POSUTM_t;
		27
		28	typedef struct {
		29	unsigned long itow; // time of week
		30	uint8_t gpsfix_type;// 3=3D Fix
		31	uint8_t nav_status_flag;
		32	uint8_t nav_diff_status;
		33	uint8_t nav_resevd;
		34	long nav_tff; // Time to First Fix in ms
		35	long nav_msss; // ms since startup
		36	uint8_t status; // 0: kein gueltiges Paket 1: alles ok
		37	} NAV_STATUS_t;
		38
		39	typedef struct {
		40	unsigned long itow;
		41	long speed_n; // in cm/s
		42	long speed_e; // in cm/s
		43	long speed_alt; // in cm/s
		44	unsigned long speed_3d; // in cm/s
		45	unsigned long speed_gnd; // V ueber Grund in cm/s
		46	long heading; // Richtung in deg/10000
		47	unsigned long sacc; // Speed Genauigkeit in cm/s
		48	unsigned long cacc; // Richtungsgenauigkeit in deg
		49	uint8_t status; // 0: kein gueltiges Paket 1: alles ok
		50	} NAV_VELNED_t;
		51
		52
		53	typedef struct {
		54	long utm_east; // UTM Ost in 10 cm
		55	long utm_north; // UTM Nord in 10 cm
		56	long utm_alt; // hoehe in 10 cm
		57	unsigned long speed_gnd; // V ueber Grund in 10cm/s
		58	unsigned heading; // Richtung in Grad
		59	uint8_t status; // 0: keine gueltigen Daten 1: alles ok
		60
		61	} GPS_ABS_POSITION_t;
		62
		63
		64	typedef struct { // Struktur fuer Relative GPS Daten (bezogen z.B. auf Home Position)
		65	int utm_east; // UTM Ost in 10 cm
		66	int utm_north; // UTM Nord in 10 cm
565	salvo	67	int utm_alt ; // UTM Altitude in 10 cm
528	salvo	68	uint8_t status; // 0: keine gueltigen Daten 1: alles ok
		69
		70	} GPS_REL_POSITION_t;
		71
		72
		73	extern GPS_ABS_POSITION_t gps_act_position;
		74	extern GPS_ABS_POSITION_t gps_home_position;
		75	extern GPS_REL_POSITION_t gps_rel_act_position;
		76	extern GPS_REL_POSITION_t gps_rel_hold_position;
		77	extern short int gps_state,gps_sub_state;
		78	extern signed int GPS_hdng_abs_2trgt;
		79	extern signed int GPS_hdng_rel_2trgt;
		80	extern signed int GPS_dist_2trgt;
		81	extern signed int gps_reg_x,gps_reg_y;
		82	extern signed int GPS_dist_2trgt;
		83	extern unsigned int gps_alive_cnt;
602	salvo	84	extern unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators
		85	extern signed int int_east,int_north; //Integrierer
615	salvo	86	//extern signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert
613	salvo	87	signed long dist;
528	salvo	88
602	salvo	89
528	salvo	90	//nur provisorisch fuer Debugausgaben
		91	extern signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
		92	extern long int dist_flown;
		93
542	salvo	94
615	salvo	95	#define GPS_USR_PAR_FKT 8 //Faktor durch den die Userparameter geteilt werden
542	salvo	96
		97
528	salvo	98	// Zustaende der zentralen GPS statemachine
		99	#define GPS_CRTL_IDLE 0 //
		100	#define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv
		101	#define GPS_CRTL_HOME_ACTIVE 2 // Rueckflug zur Basis Aktiv
		102	#define GPS_HOME_FAST_IN_TOL 3 // Rueckflug: Aktuelle Position innerhalb der Toleranz
		103	#define GPS_HOME_FAST_OUTOF_TOL 4 // Rueckflug: Aktuelle Position ausserhalb der Toleranz
		104	#define GPS_HOME_RMPDWN_IN_TOL 5 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz
		105	#define GPS_HOME_RMPDWN_OUTOF_TOL 6 // Rueckflug: beim Abbremsen Position ausserhalb der Toleranz
		106	#define GPS_HOME_IN_TOL 7 // Rueckflug: Nahe am Ziel innerhalb der Toleranz
		107	#define GPS_HOME_OUTOF_TOL 8 // Rueckflug: Nahe am Ziel ausserhalb der Toleranz
		108	#define GPS_HOME_FINISHED 9 // Rueckflug zur Basis abgeschlossen
		109
		110	// Kommandokonstanten fuer die zentrale GPS Statemachine
		111	#define GPS_CMD_STOP 0 // Lageregelung soll deaktiviert werden
		112	#define GPS_CMD_REQ_INIT 1 // Initialisierung
		113	#define GPS_CMD_REQ_HOLD 3 // Lageregelung soll aktiviert werden
		114	#define GPS_CMD_REQ_HOME 4 // Das Heimfliegen soll aktiviert werden
		115
		116	// Statusmeldungen der zentralen GPS statemachine
		117	#define GPS_STST_OK 0 // Kommando erfolgreich und abgeschlossen
		118	#define GPS_STST_PEND 1 // Kommando noch nicht komplett durchgefuehrt
		119	#define GPS_STST_ERR 2 // Fehler
		120
		121	// GPS Lageregler
616	salvo	122	#define GPS_NICKROLL_MAX 30 // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll
600	salvo	123	#define GPS_DIST_MAX 200 // Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm)
565	salvo	124	#define GPS_STICK_HOLDOFF 10 // Wenn der Nick oder Roll Stickwerte groesser sind, wird GPS_HOLD deaktiviert
528	salvo	125	#define GPS_V 8 // Teilerfaktor Regelabweichung zu Ausgabewert
		126
		127	// Differenzierer Kennwerte fuer von Distanz abhaengige Verstaerkung, abhaengig vom Modus.
		128	#define GPS_DIFF_NRML_MAX_V 20 //maximale Verstaerkung * 10
		129	#define GPS_DIFF_NRML_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
		130	#define GPS_DIFF_FAST_MAX_V 15 //maximale Verstaerkung * 10 im Fast mode
		131	#define GPS_DIFF_FAST_MAX_D 50 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm im Fast mode
615	salvo	132	// Konstanten fuer Verstaerkung fuer Differenzierer in Abhaengigkeit vom Diff Wert
		133	// um eine exponentielle Verstaerkung zu erreichen
		134	#define DIFF_Y_MAX 3 // Verstaerkung bei Eingangswert = DIFF_X_MAX
		135	#define DIFF_X_MAX 10 // bei diesem Eingangswert ist die Verstaerkung = DIFF_Y_MAX
613	salvo	136
528	salvo	137	// P-Regler Verstaerkung
		138	#define GPS_PROP_NRML_V 2 //maximale Verstaerkung * 2
		139	#define GPS_PROP_FAST_V 2 //maximale Verstaerkung * 2 im Fast mode
		140
		141	// GPS G2T /Go to Target Regler
		142	#define GPS_G2T_DIST_MAX_STOP 50 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm)
		143	#define GPS_G2T_DIST_HOLD 30 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler Geschwindigkeit eingeregelt
		144	#define GPS_G2T_V_MAX 12 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird.
		145	#define GPS_G2T_V_RAMP_DWN 6 // Geschwindigkeit (in 10cm/0.25ekunden) in der Naehe der Home Position um abzubremsen
		146	#define GPS_G2T_V_MIN 3 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition.
		147	#define GPS_G2T_FAST_TOL 60 // Bei grosser Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht
		148	#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
		149
		150

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/gps.h - Rev 619