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

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c @ 293 - Rev 158