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

Subversion Repositories FlightCtrl

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