WebSVN - FlightCtrl - Blame - Rev 159 - /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
		15	Regelung fuer GPS noch nicht implementiert
158	salvo	16	Stand 16.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	short int n;
		76	ublox_msg_state = UBLOX_IDLE;
		77	gps_state = GPS_CRTL_IDLE;
		78	actual_pos.status = 0;
		79	actual_speed.status = 0;
		80	actual_status.status = 0;
		81	gps_home_position.status = 0; // Noch keine gueltige Home Position
		82	gps_act_position.status = 0;
		83	gps_rel_act_position.status = 0;
		84	GPS_Nick = 0;
		85	GPS_Roll = 0;
		86	gps_updte_flag = 0;
159	salvo	87	gps_int_x = 0;
		88	gps_int_y = 0;
		89
1	ingob	90	}
		91
156	salvo	92	// Home Position sichern falls Daten verfuegbar sind.
		93	void GPS_Save_Home(void)
1	ingob	94	{
156	salvo	95	short int n;
		96	n = Get_GPS_data();
		97	if (n == 0) // Gueltige und aktuelle Daten ?
		98	{
		99	// Neue GPS Daten liegen vor
157	salvo	100	gps_home_position.utm_east = gps_act_position.utm_east;
		101	gps_home_position.utm_north = gps_act_position.utm_north;
		102	gps_home_position.utm_alt = gps_act_position.utm_alt;
		103	gps_home_position.status = 1; // Home Position gueltig
156	salvo	104	}
		105	}
		106
		107	// Relative Position zur Home Position bestimmen
		108	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
		109	short int Get_Rel_Position(void)
		110	{
		111	short int n = 0;
		112	n = Get_GPS_data();
		113	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
		114	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
		115	{
		116	gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);
		117	gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);
		118	gps_rel_act_position.status = 1; // gueltige Positionsdaten
		119	n = 0;
158	salvo	120	gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
156	salvo	121	}
		122	else
		123	{
		124	n = 2; //keine gueltigen Daten vorhanden
		125	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
		126	}
		127	return (n);
		128	}
		129
		130	// Daten aus aktuellen ublox Messages extrahieren
		131	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
		132	short int Get_GPS_data(void)
		133	{
		134	short int n = 1;
		135
		136	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
143	salvo	137	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
		138	{
		139	cnt1++; //**** noch Rausschmeissen
149	salvo	140	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
143	salvo	141	{
		142	gps_act_position.utm_east = actual_pos.utm_east/10;
		143	gps_act_position.utm_north = actual_pos.utm_north/10;
		144	gps_act_position.utm_alt = actual_pos.utm_alt/10;
		145	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		146	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		147	gps_act_position.heading = actual_speed.heading/100000;
		148	gps_act_position.status = 1;
156	salvo	149	n = 0; //Daten gueltig
143	salvo	150	}
156	salvo	151	else
		152	{
		153	gps_act_position.status = 0; //Keine gueltigen Daten
		154	n = 2;
		155	}
		156	actual_pos.status = 0; //neue ublox Messages anfordern
		157	actual_status.status = 0;
		158	actual_speed.status = 0;
		159	}
		160	return (n);
1	ingob	161	}
		162
		163
143	salvo	164	/*
		165	Daten vom GPS im ublox MSG Format auswerten
156	salvo	166	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
143	salvo	167	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
		168	*/
		169	void Get_Ublox_Msg(uint8_t rx)
		170	{
1	ingob	171
143	salvo	172	switch (ublox_msg_state)
		173	{
1	ingob	174
143	salvo	175	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
		176	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
		177	else ublox_msg_state = UBLOX_IDLE;
		178	break;
		179
		180	case UBLOX_SYNC1:
		181
		182	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
		183	else ublox_msg_state = UBLOX_IDLE;
		184	chk_a = 0,chk_b = 0;
		185	break;
		186
		187	case UBLOX_SYNC2:
		188	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;
		189	else ublox_msg_state = UBLOX_IDLE;
		190	break;
		191
		192	case UBLOX_CLASS: // Nur NAV Meldungen auswerten
		193	switch (rx)
		194	{
		195	case UBLOX_NAV_POSUTM:
		196	ptr_pac_status = &actual_pos.status;
		197	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
		198	else
		199	{
		200	ptr_payload_data = &actual_pos;
		201	ptr_payload_data_end = &actual_pos.status;
		202	ublox_msg_state = UBLOX_LEN1;
		203	}
		204	break;
		205
		206	case UBLOX_NAV_STATUS:
		207	ptr_pac_status = &actual_status.status;
		208	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		209	else
		210	{
		211	ptr_payload_data = &actual_status;
		212	ptr_payload_data_end = &actual_status.status;
		213	ublox_msg_state = UBLOX_LEN1;
		214	}
		215	break;
		216
		217	case UBLOX_NAV_VELED:
156	salvo	218	ptr_pac_status = &actual_speed.status;
143	salvo	219	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		220	else
		221	{
		222	ptr_payload_data = &actual_speed;
		223	ptr_payload_data_end = &actual_speed.status;
		224	ublox_msg_state = UBLOX_LEN1;
		225	}
		226	break;
		227
		228	default:
		229	ublox_msg_state = UBLOX_IDLE;
		230	break;
		231	}
		232	chk_a = UBLOX_NAV_CLASS + rx;
		233	chk_b = UBLOX_NAV_CLASS + chk_a;
		234	break;
		235
		236	case UBLOX_LEN1: // Laenge auswerten
		237	rx_len = rx;
		238	chk_a += rx;
		239	chk_b += chk_a;
		240	ublox_msg_state = UBLOX_LEN2;
		241	break;
		242
		243
		244	case UBLOX_LEN2: // Laenge auswerten
		245	rx_len = rx_len + (rx *256); // Laenge ermitteln
		246	chk_a += rx;
		247	chk_b += chk_a;
		248	ublox_msg_state = UBLOX_PAYLOAD;
		249	break;
		250
		251	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
		252	if (rx_len > 0)
		253	{
		254	*ptr_payload_data = rx;
		255	chk_a += rx;
		256	chk_b += chk_a;
		257	--rx_len;
		258	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
		259	{
		260	ptr_payload_data++;
156	salvo	261	ublox_msg_state = UBLOX_PAYLOAD;
143	salvo	262	}
		263	else ublox_msg_state = UBLOX_CKA;
		264	}
		265	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		266	break;
		267
		268	case UBLOX_CKA: // Checksum pruefen
		269	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;
		270	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		271	break;
		272
		273	case UBLOX_CKB: // Checksum pruefen
		274	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
		275	ublox_msg_state = UBLOX_IDLE;
		276	break;
		277
		278	default:
		279	ublox_msg_state = UBLOX_IDLE;
		280	break;
158	salvo	281	}
143	salvo	282	}
158	salvo	283
		284	//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
		285	short int GPS_CRTL(short int cmd)
		286	{
		287	static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen
143	salvo	288
158	salvo	289	switch (cmd)
		290	{
		291	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
		292	if (gps_state != GPS_CRTL_HOLD_ACTIVE)
		293	{
		294	cnt++;
		295	if (cnt > 300) // erst nach Verzoegerung
		296	{
		297	cnt = 0;
		298	// aktuelle positionsdaten abespeichern
		299	if (gps_rel_act_position.status > 0)
		300	{
		301	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
		302	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
		303	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
		304	gps_state = GPS_CRTL_HOLD_ACTIVE;
		305	return (GPS_STST_OK);
		306	}
		307	else
		308	{
		309	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar
		310	gps_state = GPS_CRTL_IDLE;
		311	return(GPS_STST_ERR); // Keine Daten da
		312	}
		313	}
		314	else return(GPS_STST_PEND); // noch warten
		315	}
		316	break;
		317
		318	case GPS_CMD_STOP_HOLD: // Lageregelung beenden
		319	cnt = 0;
		320	GPS_Nick = 0;
		321	GPS_Roll = 0;
		322	gps_state = GPS_CRTL_IDLE;
159	salvo	323	gps_int_x = 0;
		324	gps_int_y = 0;
158	salvo	325	return (GPS_STST_OK);
		326	break;
		327
		328	default:
		329	return (GPS_STST_ERR);
		330	break;
		331	}
		332
		333	switch (gps_state)
		334	{
		335	case GPS_CRTL_IDLE:
		336	cnt = 0;
		337	return (GPS_STST_OK);
		338	break;
		339
		340	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
159	salvo	341	if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen
158	salvo	342	{
		343	gps_updte_flag = 0;
		344	// ab hier wird geregelt
		345	signed int dist_north,dist_east;
		346	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;
		347	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
159	salvo	348
		349	//PI Regler
		350	gps_int_x = gps_int_x + (dist_east * Parameter_UserParam1)/16; // Integrator
		351	gps_int_y = gps_int_y + (dist_north * Parameter_UserParam1)/16;
		352	if ((gps_int_x >= 4096) \|\| (gps_int_y >= 4096) \|\| (gps_int_x < - 4096) \|\| (gps_int_y <= -4096))
		353	{
		354	gps_int_x -= (dist_east * Parameter_UserParam1)/16; // Integrator stoppen
		355	gps_int_y -= (dist_north * Parameter_UserParam1)/16;
		356	}
		357	gps_reg_x = gps_int_x + (dist_east * Parameter_UserParam2)/16; // P Anteil dazu
		358	gps_reg_y = gps_int_y + (dist_north * Parameter_UserParam2)/16;
		359
		360	//Richtung bezogen auf Nordpol bestimmen
		361	GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y);
		362
158	salvo	363	//in Winkel 0..360 grad umrechnen
		364	if ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
		365	else if ((dist_east < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
159	salvo	366	// Relative Richtung in bezug auf Nordachse des Kopters errechen
		367	int n,m;
		368	n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
		369	GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n;
		370	if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180))GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
		371	else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
		372	else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
		373
158	salvo	374	// Distanz zum Ziel ermitteln
		375	GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt
		376	return (GPS_STST_OK);
		377	}
		378	else return (GPS_STST_OK);
		379	break;
		380
		381	default:
		382	gps_state = GPS_CRTL_IDLE;
		383	return (GPS_STST_ERR);
		384	break;
		385	}
		386	return (GPS_STST_ERR);
		387
		388	}
		389

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c - Rev 159