WebSVN - FlightCtrl - Blame - Rev 157 - /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
157	salvo	16	Stand 15.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;
143	salvo	44	static uint8_t ublox_msg_state = UBLOX_IDLE;
		45	static uint8_t chk_a =0; //Checksum
		46	static uint8_t chk_b =0;
		47
		48	static unsigned int rx_len;
		49	unsigned int cnt0,cnt1,cnt2; //******Provisorisch
		50	static unsigned int ptr_payload_data_end;
		51
		52	static uint8_t *ptr_payload_data;
		53	static uint8_t *ptr_pac_status;
		54
156	salvo	55	short int Get_GPS_data(void);
143	salvo	56
		57	NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
		58	NAV_STATUS_t actual_status; // Aktueller Nav Status
		59	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten
		60
156	salvo	61	GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
		62	GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt
		63	GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position
143	salvo	64
156	salvo	65	// Initialisierung
		66	void GPS_Neutral(void)
1	ingob	67	{
157	salvo	68	ublox_msg_state = UBLOX_IDLE;
		69	actual_pos.status = 0;
		70	actual_speed.status = 0;
		71	actual_status.status = 0;
		72	gps_home_position.status = 0; // Noch keine gueltige Home Position
		73	gps_act_position.status = 0;
		74	gps_rel_act_position.status = 0;
		75
1	ingob	76	}
		77
156	salvo	78	// Home Position sichern falls Daten verfuegbar sind.
		79	void GPS_Save_Home(void)
1	ingob	80	{
156	salvo	81	short int n;
		82	n = Get_GPS_data();
		83	if (n == 0) // Gueltige und aktuelle Daten ?
		84	{
		85	// Neue GPS Daten liegen vor
157	salvo	86	gps_home_position.utm_east = gps_act_position.utm_east;
		87	gps_home_position.utm_north = gps_act_position.utm_north;
		88	gps_home_position.utm_alt = gps_act_position.utm_alt;
		89	gps_home_position.status = 1; // Home Position gueltig
156	salvo	90	}
		91	}
		92
		93	// Relative Position zur Home Position bestimmen
		94	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
		95	short int Get_Rel_Position(void)
		96	{
		97	short int n = 0;
		98	n = Get_GPS_data();
		99	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
		100	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
		101	{
		102	gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);
		103	gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);
		104	gps_rel_act_position.status = 1; // gueltige Positionsdaten
		105	n = 0;
		106	}
		107	else
		108	{
		109	n = 2; //keine gueltigen Daten vorhanden
		110	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
		111	}
		112	return (n);
		113	}
		114
		115	// Daten aus aktuellen ublox Messages extrahieren
		116	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
		117	short int Get_GPS_data(void)
		118	{
		119	short int n = 1;
		120
		121	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
143	salvo	122	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
		123	{
		124	cnt1++; //**** noch Rausschmeissen
149	salvo	125	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
143	salvo	126	{
		127	gps_act_position.utm_east = actual_pos.utm_east/10;
		128	gps_act_position.utm_north = actual_pos.utm_north/10;
		129	gps_act_position.utm_alt = actual_pos.utm_alt/10;
		130	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		131	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		132	gps_act_position.heading = actual_speed.heading/100000;
		133	gps_act_position.status = 1;
156	salvo	134	n = 0; //Daten gueltig
143	salvo	135	}
156	salvo	136	else
		137	{
		138	gps_act_position.status = 0; //Keine gueltigen Daten
		139	n = 2;
		140	}
		141	actual_pos.status = 0; //neue ublox Messages anfordern
		142	actual_status.status = 0;
		143	actual_speed.status = 0;
		144	}
		145	return (n);
1	ingob	146	}
		147
		148
143	salvo	149	/*
		150	Daten vom GPS im ublox MSG Format auswerten
156	salvo	151	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
143	salvo	152	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
		153	*/
		154	void Get_Ublox_Msg(uint8_t rx)
		155	{
1	ingob	156
143	salvo	157	switch (ublox_msg_state)
		158	{
1	ingob	159
143	salvo	160	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
		161	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
		162	else ublox_msg_state = UBLOX_IDLE;
		163	break;
		164
		165	case UBLOX_SYNC1:
		166
		167	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
		168	else ublox_msg_state = UBLOX_IDLE;
		169	chk_a = 0,chk_b = 0;
		170	break;
		171
		172	case UBLOX_SYNC2:
		173	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;
		174	else ublox_msg_state = UBLOX_IDLE;
		175	break;
		176
		177	case UBLOX_CLASS: // Nur NAV Meldungen auswerten
		178	switch (rx)
		179	{
		180	case UBLOX_NAV_POSUTM:
		181	ptr_pac_status = &actual_pos.status;
		182	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
		183	else
		184	{
		185	ptr_payload_data = &actual_pos;
		186	ptr_payload_data_end = &actual_pos.status;
		187	ublox_msg_state = UBLOX_LEN1;
		188	}
		189	break;
		190
		191	case UBLOX_NAV_STATUS:
		192	ptr_pac_status = &actual_status.status;
		193	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		194	else
		195	{
		196	ptr_payload_data = &actual_status;
		197	ptr_payload_data_end = &actual_status.status;
		198	ublox_msg_state = UBLOX_LEN1;
		199	}
		200	break;
		201
		202	case UBLOX_NAV_VELED:
156	salvo	203	ptr_pac_status = &actual_speed.status;
143	salvo	204	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		205	else
		206	{
		207	ptr_payload_data = &actual_speed;
		208	ptr_payload_data_end = &actual_speed.status;
		209	ublox_msg_state = UBLOX_LEN1;
		210	}
		211	break;
		212
		213	default:
		214	ublox_msg_state = UBLOX_IDLE;
		215	break;
		216	}
		217	chk_a = UBLOX_NAV_CLASS + rx;
		218	chk_b = UBLOX_NAV_CLASS + chk_a;
		219	break;
		220
		221	case UBLOX_LEN1: // Laenge auswerten
		222	rx_len = rx;
		223	chk_a += rx;
		224	chk_b += chk_a;
		225	ublox_msg_state = UBLOX_LEN2;
		226	break;
		227
		228
		229	case UBLOX_LEN2: // Laenge auswerten
		230	rx_len = rx_len + (rx *256); // Laenge ermitteln
		231	chk_a += rx;
		232	chk_b += chk_a;
		233	ublox_msg_state = UBLOX_PAYLOAD;
		234	break;
		235
		236	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
		237	if (rx_len > 0)
		238	{
		239	*ptr_payload_data = rx;
		240	chk_a += rx;
		241	chk_b += chk_a;
		242	--rx_len;
		243	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
		244	{
		245	ptr_payload_data++;
156	salvo	246	ublox_msg_state = UBLOX_PAYLOAD;
143	salvo	247	}
		248	else ublox_msg_state = UBLOX_CKA;
		249	}
		250	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		251	break;
		252
		253	case UBLOX_CKA: // Checksum pruefen
		254	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;
		255	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		256	break;
		257
		258	case UBLOX_CKB: // Checksum pruefen
		259	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
		260	ublox_msg_state = UBLOX_IDLE;
		261	break;
		262
		263	default:
		264	ublox_msg_state = UBLOX_IDLE;
		265	break;
		266	}
		267	}
		268

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c @ 354 - Rev 157