WebSVN - FlightCtrl - Blame - Rev 182 - /branches/salvo

Rev	Author	Line No.	Line
143	salvo	1	/*
		2	This program (files gps.c and gps.h) is free software; you can redistribute it and/or modify
		3	it under the terms of the GNU General Public License as published by the Free Software Foundation;
		4	either version 3 of the License, or (at your option) any later version.
		5	This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
		6	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		7	GNU General Public License for more details. You should have received a copy of the GNU General Public License
		8	along with this program. If not, see <http://www.gnu.org/licenses/>.
		9
149	salvo	10	Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de
143	salvo	11	*/
		12	/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		13	Peter Muehlenbrock
		14	Auswertung der Daten vom GPS im ublox Format
182	salvo	15	Hold Modus mit PID Regler
		16	Stand 27.9.2007
143	salvo	17	++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		18	*/
1	ingob	19	#include "main.h"
156	salvo	20	//#include "gps.h"
1	ingob	21
143	salvo	22	// Defines fuer ublox Messageformat um Auswertung zu steuern
		23	#define UBLOX_IDLE 0
		24	#define UBLOX_SYNC1 1
		25	#define UBLOX_SYNC2 2
		26	#define UBLOX_CLASS 3
		27	#define UBLOX_ID 4
		28	#define UBLOX_LEN1 5
		29	#define UBLOX_LEN2 6
		30	#define UBLOX_CKA 7
		31	#define UBLOX_CKB 8
		32	#define UBLOX_PAYLOAD 9
		33
		34	// ublox Protokoll Identifier
		35	#define UBLOX_NAV_POSUTM 0x08
		36	#define UBLOX_NAV_STATUS 0x03
		37	#define UBLOX_NAV_VELED 0x12
		38	#define UBLOX_NAV_CLASS 0x01
		39	#define UBLOX_SYNCH1_CHAR 0xB5
		40	#define UBLOX_SYNCH2_CHAR 0x62
		41
1	ingob	42	signed int GPS_Nick = 0;
		43	signed int GPS_Roll = 0;
158	salvo	44	short int ublox_msg_state = UBLOX_IDLE;
143	salvo	45	static uint8_t chk_a =0; //Checksum
		46	static uint8_t chk_b =0;
158	salvo	47	short int gps_state;
		48	short int gps_updte_flag;
		49	signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
		50	signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
		51	signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
159	salvo	52	signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
		53	signed int ;
143	salvo	54	static unsigned int rx_len;
		55	unsigned int cnt0,cnt1,cnt2; //******Provisorisch
		56	static unsigned int ptr_payload_data_end;
		57
		58	static uint8_t *ptr_payload_data;
		59	static uint8_t *ptr_pac_status;
		60
156	salvo	61	short int Get_GPS_data(void);
143	salvo	62
		63	NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
		64	NAV_STATUS_t actual_status; // Aktueller Nav Status
		65	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten
		66
156	salvo	67	GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
		68	GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt
		69	GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position
158	salvo	70	GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode
143	salvo	71
156	salvo	72	// Initialisierung
		73	void GPS_Neutral(void)
1	ingob	74	{
158	salvo	75	ublox_msg_state = UBLOX_IDLE;
		76	gps_state = GPS_CRTL_IDLE;
		77	actual_pos.status = 0;
		78	actual_speed.status = 0;
		79	actual_status.status = 0;
		80	gps_home_position.status = 0; // Noch keine gueltige Home Position
		81	gps_act_position.status = 0;
		82	gps_rel_act_position.status = 0;
		83	GPS_Nick = 0;
		84	GPS_Roll = 0;
		85	gps_updte_flag = 0;
159	salvo	86	gps_int_x = 0;
		87	gps_int_y = 0;
		88
1	ingob	89	}
		90
156	salvo	91	// Home Position sichern falls Daten verfuegbar sind.
		92	void GPS_Save_Home(void)
1	ingob	93	{
156	salvo	94	short int n;
		95	n = Get_GPS_data();
		96	if (n == 0) // Gueltige und aktuelle Daten ?
		97	{
		98	// Neue GPS Daten liegen vor
157	salvo	99	gps_home_position.utm_east = gps_act_position.utm_east;
		100	gps_home_position.utm_north = gps_act_position.utm_north;
		101	gps_home_position.utm_alt = gps_act_position.utm_alt;
		102	gps_home_position.status = 1; // Home Position gueltig
156	salvo	103	}
		104	}
		105
		106	// Relative Position zur Home Position bestimmen
		107	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
		108	short int Get_Rel_Position(void)
		109	{
		110	short int n = 0;
		111	n = Get_GPS_data();
		112	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
		113	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
		114	{
		115	gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);
		116	gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);
		117	gps_rel_act_position.status = 1; // gueltige Positionsdaten
		118	n = 0;
158	salvo	119	gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
156	salvo	120	}
		121	else
		122	{
		123	n = 2; //keine gueltigen Daten vorhanden
		124	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
		125	}
		126	return (n);
		127	}
		128
		129	// Daten aus aktuellen ublox Messages extrahieren
		130	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
		131	short int Get_GPS_data(void)
		132	{
		133	short int n = 1;
		134
		135	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
143	salvo	136	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
		137	{
		138	cnt1++; //**** noch Rausschmeissen
149	salvo	139	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
143	salvo	140	{
		141	gps_act_position.utm_east = actual_pos.utm_east/10;
		142	gps_act_position.utm_north = actual_pos.utm_north/10;
		143	gps_act_position.utm_alt = actual_pos.utm_alt/10;
		144	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		145	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		146	gps_act_position.heading = actual_speed.heading/100000;
		147	gps_act_position.status = 1;
156	salvo	148	n = 0; //Daten gueltig
143	salvo	149	}
156	salvo	150	else
		151	{
		152	gps_act_position.status = 0; //Keine gueltigen Daten
		153	n = 2;
		154	}
		155	actual_pos.status = 0; //neue ublox Messages anfordern
		156	actual_status.status = 0;
		157	actual_speed.status = 0;
		158	}
		159	return (n);
1	ingob	160	}
		161
		162
143	salvo	163	/*
		164	Daten vom GPS im ublox MSG Format auswerten
156	salvo	165	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
143	salvo	166	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
		167	*/
		168	void Get_Ublox_Msg(uint8_t rx)
		169	{
1	ingob	170
143	salvo	171	switch (ublox_msg_state)
		172	{
1	ingob	173
143	salvo	174	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
		175	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
		176	else ublox_msg_state = UBLOX_IDLE;
		177	break;
		178
		179	case UBLOX_SYNC1:
		180
		181	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
		182	else ublox_msg_state = UBLOX_IDLE;
		183	chk_a = 0,chk_b = 0;
		184	break;
		185
		186	case UBLOX_SYNC2:
		187	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;
		188	else ublox_msg_state = UBLOX_IDLE;
		189	break;
		190
		191	case UBLOX_CLASS: // Nur NAV Meldungen auswerten
		192	switch (rx)
		193	{
		194	case UBLOX_NAV_POSUTM:
		195	ptr_pac_status = &actual_pos.status;
		196	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
		197	else
		198	{
		199	ptr_payload_data = &actual_pos;
		200	ptr_payload_data_end = &actual_pos.status;
		201	ublox_msg_state = UBLOX_LEN1;
		202	}
		203	break;
		204
		205	case UBLOX_NAV_STATUS:
		206	ptr_pac_status = &actual_status.status;
		207	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		208	else
		209	{
		210	ptr_payload_data = &actual_status;
		211	ptr_payload_data_end = &actual_status.status;
		212	ublox_msg_state = UBLOX_LEN1;
		213	}
		214	break;
		215
		216	case UBLOX_NAV_VELED:
156	salvo	217	ptr_pac_status = &actual_speed.status;
143	salvo	218	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		219	else
		220	{
		221	ptr_payload_data = &actual_speed;
		222	ptr_payload_data_end = &actual_speed.status;
		223	ublox_msg_state = UBLOX_LEN1;
		224	}
		225	break;
		226
		227	default:
		228	ublox_msg_state = UBLOX_IDLE;
		229	break;
		230	}
		231	chk_a = UBLOX_NAV_CLASS + rx;
		232	chk_b = UBLOX_NAV_CLASS + chk_a;
		233	break;
		234
		235	case UBLOX_LEN1: // Laenge auswerten
		236	rx_len = rx;
		237	chk_a += rx;
		238	chk_b += chk_a;
		239	ublox_msg_state = UBLOX_LEN2;
		240	break;
		241
		242
		243	case UBLOX_LEN2: // Laenge auswerten
		244	rx_len = rx_len + (rx *256); // Laenge ermitteln
		245	chk_a += rx;
		246	chk_b += chk_a;
		247	ublox_msg_state = UBLOX_PAYLOAD;
		248	break;
		249
		250	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
		251	if (rx_len > 0)
		252	{
		253	*ptr_payload_data = rx;
		254	chk_a += rx;
		255	chk_b += chk_a;
		256	--rx_len;
		257	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
		258	{
		259	ptr_payload_data++;
156	salvo	260	ublox_msg_state = UBLOX_PAYLOAD;
143	salvo	261	}
		262	else ublox_msg_state = UBLOX_CKA;
		263	}
		264	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		265	break;
		266
		267	case UBLOX_CKA: // Checksum pruefen
		268	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;
		269	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		270	break;
		271
		272	case UBLOX_CKB: // Checksum pruefen
		273	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
		274	ublox_msg_state = UBLOX_IDLE;
		275	break;
		276
		277	default:
		278	ublox_msg_state = UBLOX_IDLE;
		279	break;
158	salvo	280	}
143	salvo	281	}
158	salvo	282
		283	//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
		284	short int GPS_CRTL(short int cmd)
		285	{
167	salvo	286	static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen
162	salvo	287	static int int_east,int_north; //Integrierer
167	salvo	288	static signed int dist_north,dist_east;
		289	int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert)
160	salvo	290	int n;
		291	long int dist;
158	salvo	292	switch (cmd)
		293	{
162	salvo	294
		295	case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
		296	// Noch einiges zu ueberlegen und zu tun
		297	return(GPS_STST_PEND); // noch warten
		298	break;
		299	// ******************************
		300
158	salvo	301	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
		302	if (gps_state != GPS_CRTL_HOLD_ACTIVE)
		303	{
		304	cnt++;
160	salvo	305	if (cnt > 500) // erst nach Verzoegerung
158	salvo	306	{
		307	cnt = 0;
		308	// aktuelle positionsdaten abespeichern
		309	if (gps_rel_act_position.status > 0)
		310	{
162	salvo	311	int_east = 0;
		312	int_north = 0;
		313	gps_reg_x = 0;
		314	gps_reg_y = 0;
158	salvo	315	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
		316	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
		317	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
		318	gps_state = GPS_CRTL_HOLD_ACTIVE;
		319	return (GPS_STST_OK);
		320	}
		321	else
		322	{
		323	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar
		324	gps_state = GPS_CRTL_IDLE;
		325	return(GPS_STST_ERR); // Keine Daten da
		326	}
		327	}
		328	else return(GPS_STST_PEND); // noch warten
		329	}
		330	break;
		331
162	salvo	332	case GPS_CMD_STOP: // Lageregelung beenden
		333	cnt = 0;
		334	GPS_Nick = 0;
		335	GPS_Roll = 0;
		336	gps_int_x = 0;
		337	gps_int_y = 0;
		338	gps_state = GPS_CRTL_IDLE;
158	salvo	339	return (GPS_STST_OK);
		340	break;
		341
		342	default:
		343	return (GPS_STST_ERR);
		344	break;
		345	}
		346
		347	switch (gps_state)
		348	{
		349	case GPS_CRTL_IDLE:
		350	cnt = 0;
		351	return (GPS_STST_OK);
		352	break;
		353
167	salvo	354	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
159	salvo	355	if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen
158	salvo	356	{
		357	gps_updte_flag = 0;
		358	// ab hier wird geregelt
167	salvo	359
		360	diff_east = -dist_east; //Alten Wert schon mal abziehen
		361	diff_north = -dist_north;
158	salvo	362	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;
		363	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
162	salvo	364	int_east += dist_east;
		365	int_north += dist_north;
167	salvo	366	diff_east += dist_east; // Differenz zur vorhergehenden East Position
		367	diff_north += dist_north; // Differenz zur vorhergehenden North Position
161	salvo	368
167	salvo	369	#define GPSINT_MAX 2048 //neuer Wert ab 25.9.2007 Begrenzung
162	salvo	370	if ((abs(int_east) > GPSINT_MAX) \|\| (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
		371	{
		372	int_east -= dist_east;
		373	int_north -= dist_north;
		374	}
		375
167	salvo	376	#define DIST_MAX 100 //neuer Wert ab 24.9.2007 Begrenzung
162	salvo	377	if (dist_east > DIST_MAX) dist_east = DIST_MAX;
		378	if (dist_east <-DIST_MAX) dist_east = -DIST_MAX;
		379	if (dist_north > DIST_MAX) dist_north = DIST_MAX;
		380	if (dist_north <-DIST_MAX) dist_north = -DIST_MAX;
161	salvo	381
167	salvo	382	//PID Regler
182	salvo	383	gps_reg_x = ((int_east * Parameter_UserParam1)/32) + ((dist_east * Parameter_UserParam2)/8)+ ((diff_east * Parameter_UserParam3)/2); //
		384	gps_reg_y = ((int_north * Parameter_UserParam1)/32) + ((dist_north * Parameter_UserParam2)/8)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse
159	salvo	385
		386	//Richtung bezogen auf Nordpol bestimmen
		387	GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y);
		388
158	salvo	389	//in Winkel 0..360 grad umrechnen
161	salvo	390	if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
162	salvo	391	else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
161	salvo	392
159	salvo	393	// Relative Richtung in bezug auf Nordachse des Kopters errechen
		394	n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
		395	GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n;
167	salvo	396	if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
159	salvo	397	else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
		398	else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
		399
160	salvo	400	// Regelabweichung aus x,y zu Ziel in Distanz umrechnen
		401
		402	if (abs(gps_reg_x) > abs(gps_reg_y) )
		403	{
182	salvo	404	dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
161	salvo	405	dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
160	salvo	406	}
		407	else
		408	{
182	salvo	409	dist = (long)gps_reg_y;
161	salvo	410	dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
160	salvo	411	}
161	salvo	412	if (dist > 200) dist = 200;
160	salvo	413	GPS_dist_2trgt = (int )dist;
		414
		415	// Winkel und Distanz in Nick und Roll groessen umrechnen
		416	long int a,b;
182	salvo	417	GPS_Roll = (int) +((dist * sin_i(GPS_hdng_rel_2trgt))/(1000*8));
		418	GPS_Nick = (int) -((dist * cos_i(GPS_hdng_rel_2trgt))/(1000*8));
160	salvo	419
161	salvo	420	if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen
		421	else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX;
		422	if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX;
		423	else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX;
158	salvo	424	return (GPS_STST_OK);
		425	}
		426	else return (GPS_STST_OK);
		427	break;
		428
		429	default:
		430	gps_state = GPS_CRTL_IDLE;
		431	return (GPS_STST_ERR);
		432	break;
		433	}
		434	return (GPS_STST_ERR);
		435
		436	}
		437

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c @ 266 - Rev 182