WebSVN - FlightCtrl - Blame - Rev 149 - /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
149	salvo	16	Stand 11.9.2007
143	salvo	17	++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		18	*/
1	ingob	19	#include "main.h"
		20
143	salvo	21	// Defines fuer ublox Messageformat um Auswertung zu steuern
		22	#define UBLOX_IDLE 0
		23	#define UBLOX_SYNC1 1
		24	#define UBLOX_SYNC2 2
		25	#define UBLOX_CLASS 3
		26	#define UBLOX_ID 4
		27	#define UBLOX_LEN1 5
		28	#define UBLOX_LEN2 6
		29	#define UBLOX_CKA 7
		30	#define UBLOX_CKB 8
		31	#define UBLOX_PAYLOAD 9
		32
		33	// ublox Protokoll Identifier
		34	#define UBLOX_NAV_POSUTM 0x08
		35	#define UBLOX_NAV_STATUS 0x03
		36	#define UBLOX_NAV_VELED 0x12
		37	#define UBLOX_NAV_CLASS 0x01
		38	#define UBLOX_SYNCH1_CHAR 0xB5
		39	#define UBLOX_SYNCH2_CHAR 0x62
		40
1	ingob	41	signed int GPS_Nick = 0;
		42	signed int GPS_Roll = 0;
143	salvo	43	static uint8_t ublox_msg_state = UBLOX_IDLE;
		44	static uint8_t chk_a =0; //Checksum
		45	static uint8_t chk_b =0;
		46
		47	static unsigned int rx_len;
		48	unsigned int cnt0,cnt1,cnt2; //******Provisorisch
		49	static unsigned int ptr_payload_data_end;
		50
		51	static uint8_t *ptr_payload_data;
		52	static uint8_t *ptr_pac_status;
		53
		54
		55	NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
		56	NAV_STATUS_t actual_status; // Aktueller Nav Status
		57	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten
		58
		59	GPS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
		60
		61	void GPS_Neutral(void) // Initialisierung
1	ingob	62	{
143	salvo	63	ublox_msg_state = UBLOX_IDLE;
		64	actual_pos.status= 0;
		65	actual_speed.status= 0;
		66	actual_status.status= 0;
1	ingob	67	}
		68
143	salvo	69	void Get_GPS_data(void) // Daten aus aktuellen ublox Messages extrahieren
1	ingob	70	{
143	salvo	71	if (actual_pos.status == 0) return; //damit es schnell geht, wenn nix zu tun ist
		72	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
		73	{
		74	cnt1++; //**** noch Rausschmeissen
149	salvo	75	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
143	salvo	76	{
		77	gps_act_position.utm_east = actual_pos.utm_east/10;
		78	gps_act_position.utm_north = actual_pos.utm_north/10;
		79	gps_act_position.utm_alt = actual_pos.utm_alt/10;
		80	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		81	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		82	gps_act_position.heading = actual_speed.heading/100000;
		83	gps_act_position.status = 1;
		84	}
		85	actual_pos.status = 0;
		86	actual_status.status = 0;
		87	actual_speed.status = 0;
		88	}
1	ingob	89	}
		90
		91
143	salvo	92	/*
		93	Daten vom GPS im ublox MSG Format auswerten
		94	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
		95	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
		96	*/
		97	void Get_Ublox_Msg(uint8_t rx)
		98	{
1	ingob	99
143	salvo	100	switch (ublox_msg_state)
		101	{
1	ingob	102
143	salvo	103	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
		104	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
		105	else ublox_msg_state = UBLOX_IDLE;
		106	break;
		107
		108	case UBLOX_SYNC1:
		109
		110	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
		111	else ublox_msg_state = UBLOX_IDLE;
		112	chk_a = 0,chk_b = 0;
		113	break;
		114
		115	case UBLOX_SYNC2:
		116	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;
		117	else ublox_msg_state = UBLOX_IDLE;
		118	break;
		119
		120	case UBLOX_CLASS: // Nur NAV Meldungen auswerten
		121	switch (rx)
		122	{
		123	case UBLOX_NAV_POSUTM:
		124	ptr_pac_status = &actual_pos.status;
		125	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
		126	else
		127	{
		128	ptr_payload_data = &actual_pos;
		129	ptr_payload_data_end = &actual_pos.status;
		130	ublox_msg_state = UBLOX_LEN1;
		131	}
		132	break;
		133
		134	case UBLOX_NAV_STATUS:
		135	ptr_pac_status = &actual_status.status;
		136	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		137	else
		138	{
		139	ptr_payload_data = &actual_status;
		140	ptr_payload_data_end = &actual_status.status;
		141	ublox_msg_state = UBLOX_LEN1;
		142	}
		143	break;
		144
		145	case UBLOX_NAV_VELED:
		146	ptr_pac_status = &actual_speed.status;
		147	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		148	else
		149	{
		150	ptr_payload_data = &actual_speed;
		151	ptr_payload_data_end = &actual_speed.status;
		152	ublox_msg_state = UBLOX_LEN1;
		153	}
		154	break;
		155
		156	default:
		157	ublox_msg_state = UBLOX_IDLE;
		158	break;
		159	}
		160	chk_a = UBLOX_NAV_CLASS + rx;
		161	chk_b = UBLOX_NAV_CLASS + chk_a;
		162	break;
		163
		164	case UBLOX_LEN1: // Laenge auswerten
		165	rx_len = rx;
		166	chk_a += rx;
		167	chk_b += chk_a;
		168	ublox_msg_state = UBLOX_LEN2;
		169	break;
		170
		171
		172	case UBLOX_LEN2: // Laenge auswerten
		173	rx_len = rx_len + (rx *256); // Laenge ermitteln
		174	chk_a += rx;
		175	chk_b += chk_a;
		176	ublox_msg_state = UBLOX_PAYLOAD;
		177	break;
		178
		179	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
		180	if (rx_len > 0)
		181	{
		182	*ptr_payload_data = rx;
		183	chk_a += rx;
		184	chk_b += chk_a;
		185	--rx_len;
		186	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
		187	{
		188	ptr_payload_data++;
		189	ublox_msg_state = UBLOX_PAYLOAD;
		190	}
		191	else ublox_msg_state = UBLOX_CKA;
		192	}
		193	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		194	break;
		195
		196	case UBLOX_CKA: // Checksum pruefen
		197	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;
		198	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		199	break;
		200
		201	case UBLOX_CKB: // Checksum pruefen
		202	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
		203	ublox_msg_state = UBLOX_IDLE;
		204	break;
		205
		206	default:
		207	ublox_msg_state = UBLOX_IDLE;
		208	break;
		209	}
		210	}
		211

Subversion Repositories FlightCtrl

(root)/branches/salvo_gierkompass/GPS.c @ 158 - Rev 149