WebSVN - Projects - Blame - /Transportables

Rev	Author	Line No.	Line
1053	-	1	/*****************************************************************************
		2	* Copyright (C) 2010 Sebastian Boehm, seb@exse.net *
		3	* *
		4	* This program is free software; you can redistribute it and/or modify *
		5	* it under the terms of the GNU General Public License as published by *
		6	* the Free Software Foundation; either version 2 of the License. *
		7	* *
		8	* This program is distributed in the hope that it will be useful, *
		9	* but WITHOUT ANY WARRANTY; without even the implied warranty of *
		10	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
		11	* GNU General Public License for more details. *
		12	* *
		13	* You should have received a copy of the GNU General Public License *
		14	* along with this program; if not, write to the *
		15	* Free Software Foundation, Inc., *
		16	* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
		17	* *
		18	*****************************************************************************/
		19
		20
		21	#include <avr/io.h>
		22	#include <inttypes.h>
		23	#include <stdlib.h>
		24	#include <avr/pgmspace.h>
		25
		26	#include "main.h"
		27	#include "lcd.h"
		28	#include "timer.h"
		29	#include "usart.h"
		30
		31	#define TIMEOUT 200 // 2 sec
		32
		33	uint8_t ck_a = 0;
		34	uint8_t ck_b = 0;
		35	uint8_t UBX_class = 0;
		36	uint8_t UBX_id = 0;
		37	uint8_t UBX_buffer[250];
		38	uint8_t UBX_payload_counter = 0;
		39
		40	void checksum(uint8_t);
		41	void UBX_process(void);
		42	uint32_t join_4_bytes(uint8_t*);
		43
		44	uint8_t display_mode = 0;
		45
		46	void gps(void)
		47	{
		48	lcd_cls();
		49	display_mode = 2;
		50
		51	if (hardware == FC)
		52	{
		53	lcd_printp_at(0, 3, PSTR("Only with NC !"), 0);
		54	timer = 100;
		55	while (timer > 0);
		56	return;
		57	}
		58
		59
		60	if(current_hardware != NC) SwitchToNC();
		61
		62	SwitchToGPS();
		63
		64	uint8_t mode = 0;
		65
		66	// SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);
		67
		68	timer = TIMEOUT;
		69	uint8_t data = 0;
		70	uint8_t length = 0;
		71	uint8_t UBX_ck_a = 0;
		72	do
		73	{
		74	// if (rxFlag == 1)
		75	if (uart_getc_nb(&data))
		76	{
		77	//rxFlag = 0;
		78	//data = rx_byte;
		79	timer = TIMEOUT;
		80
		81	switch(mode)
		82	{
		83	case 0: // init 1
		84	if(data == 0xB5)
		85	{
		86	UBX_payload_counter = 0;
		87	UBX_id = 0;
		88	UBX_class = 0;
		89	ck_a = 0;
		90	ck_b = 0;
		91	mode++;
		92	}
		93	break;
		94	case 1: // init 2
		95	if(data == 0x62)
		96	{
		97	mode++;
		98	}
		99	else
		100	{
		101	mode = 0;
		102	}
		103	break;
		104	case 2: //class
		105	if(data != 1)
		106	{
		107	mode = 0;
		108	}
		109	else
		110	{
		111	checksum(data);
		112	UBX_class = data;
		113	mode++;
		114	}
		115	break;
		116	case 3: // id
		117	if((data != 48)&&(data != 6)&&(data != 18)&&(data != 2))
		118	{
		119	mode = 0;
		120	}
		121	else
		122	{
		123	UBX_id = data;
		124	checksum(data);
		125	mode++;
		126	}
		127	break;
		128	case 4: // length lo
		129	if(data > 250)
		130	{
		131	mode = 0;
		132	}
		133	else
		134	{
		135	checksum(data);
		136	length = data;
		137	mode++;
		138	}
		139	break;
		140	case 5: // length hi
		141	if(data != 0)
		142	{
		143	mode = 0;
		144	}
		145	else
		146	{
		147	checksum(data);
		148	mode++;
		149	}
		150	break;
		151	case 6: // length hi
		152	length--;
		153	UBX_buffer[UBX_payload_counter] = data;
		154	checksum(data);
		155	UBX_payload_counter++;
		156	if(length==0)
		157	{
		158	mode++;
		159	};
		160	break;
		161	case 7: // check lo
		162	mode++;
		163	UBX_ck_a = data;
		164	break;
		165	case 8: // check hi
		166	mode=0;
		167	if((UBX_ck_a == ck_a)&&(data == ck_b))
		168	{
		169	UBX_process();
		170	}
		171	}
		172
		173
		174	// write_ndigit_number_u (14, 0, data, 3, 0);
		175	}
		176
		177	}
		178	while (!get_key_press (1 << KEY_ESC) && timer);
		179	get_key_press(KEY_ALL);
		180
		181	SwitchToNC();
		182
		183
		184	}
		185
		186	void UBX_process()
		187	{
		188
		189	if ((get_key_press (1 << KEY_MINUS))\|\|(display_mode ==2))
		190	{
		191	if (display_mode != 1)
		192	{
		193	lcd_cls();
		194	lcd_printp_at (0,0, PSTR("Fix Type : "), 0);
		195	lcd_printp_at (0,1, PSTR("Sat : "), 0);
		196	lcd_printp_at (0,2, PSTR("Accuracy : "), 0);
		197	lcd_printp_at (0,3, PSTR("PDOP : "), 0);
		198	lcd_printp_at (0,4, PSTR("Speed : "), 0);
		199	lcd_printp_at (0,5, PSTR("Long : "), 0);
		200	lcd_printp_at (0,6, PSTR("Lat : "), 0);
		201	lcd_printp_at (0,7, PSTR("Alt : "), 0);
		202	}
		203	display_mode = 1;
		204	}
		205	if (get_key_press (1 << KEY_PLUS))
		206	{
		207	if (display_mode == 1)
		208	{
		209	lcd_cls();
		210	}
		211
		212	display_mode = 0;
		213	}
		214
		215	if((UBX_class == 1)&&(UBX_id == 48)&&(display_mode == 0))
		216	{
		217	uint8_t channels = UBX_buffer[4];
		218
		219	uint8_t i = 0;
		220	for(i = 0; i < channels; i++)
		221	{
		222	if (i > 15) break;
		223	uint8_t line;
		224	uint8_t col;
		225
		226	if(i > 7)
		227	{
		228	line = i-7;col = 11;
		229	}
		230	else
		231	{
		232	col = 0; line = i;
		233	}
		234	write_ndigit_number_u (col, line, UBX_buffer[9 + 12*i], 3, 0);
		235	write_ndigit_number_u (col+4, line, UBX_buffer[12 + 12*i], 2, 0);
		236	if((UBX_buffer[10 + 12*i] & 3) == 3)
		237	{
		238	lcd_printp_at (col+7,line, PSTR("O"), 0);
		239	}
		240	else if((UBX_buffer[10 + 12*i] & 1) == 1)
		241	{
		242	lcd_printp_at (col+7,line, PSTR("X"), 0);
		243	}
		244	else if(UBX_buffer[11 + 12*i] > 4)
		245	{
		246	lcd_printp_at (col+7,line, PSTR("x"), 0);
		247	}
		248	else if(UBX_buffer[11 + 12*i] > 1)
		249	{
		250	lcd_printp_at (col+7,line, PSTR("-"), 0);
		251	}
		252	else
		253	{
		254	lcd_printp_at (col+7,line, PSTR(" "), 0);
		255	}
		256	}
		257
		258	}
		259
		260	if(display_mode == 1)
		261	{
		262	if((UBX_class == 1)&&(UBX_id == 6))//SVINFO
		263	{
		264	switch (UBX_buffer[10])
		265	{
		266	case 4:
		267	case 3:
		268	lcd_printp_at (11,0, PSTR("3D"), 0);
		269	break;
		270	case 2:
		271	lcd_printp_at (11,0, PSTR("2D"), 0);
		272	break;
		273	default:
		274	lcd_printp_at (11,0, PSTR("no"), 0);
		275	}
		276	if((UBX_buffer[11] & 3) == 3)
		277	{
		278	lcd_printp_at (17,0, PSTR("D"), 0);
		279	}
		280	else
		281	{
		282	lcd_printp_at (17,0, PSTR(" "), 0);
		283	}
		284
		285	if((UBX_buffer[11] & 1) == 1)
		286	{
		287	lcd_printp_at (14,0, PSTR("ok"), 0);
		288	}
		289	else
		290	{
		291	lcd_printp_at (14,0, PSTR(" "), 0);
		292	}
		293	lcd_write_number_u_at (11, 1, UBX_buffer[47]);
		294	uint16_t pdop = UBX_buffer[44]+UBX_buffer[45]*255;
		295	write_ndigit_number_u (11, 3, pdop/100, 2, 0);
		296	lcd_printp_at (13,3, PSTR("."), 0);
		297	write_ndigit_number_u (14, 3, (pdop % 100),2, 1);
		298
		299	uint16_t acc = (uint16_t)join_4_bytes(&UBX_buffer[24]);
		300	write_ndigit_number_u (11, 2, acc, 5, 0);
		301	lcd_printp_at (17,2, PSTR("cm"), 0);
		302	}
		303	if((UBX_class == 1)&&(UBX_id == 18))//VELNED
		304	{
		305	uint16_t speed = (uint16_t)((join_4_bytes(&UBX_buffer[20])6060)/100000);
		306	write_ndigit_number_u (11, 4, speed, 3, 0);
		307	lcd_printp_at (15,4, PSTR("km/h"), 0);
		308
		309	}
		310	if((UBX_class == 1)&&(UBX_id == 2))//POSLLH
		311	{
		312	uint16_t height = (uint16_t)(join_4_bytes(&UBX_buffer[16])/1000);
		313	write_ndigit_number_u (11, 7, height, 4, 0);
		314	lcd_printp_at (16,7, PSTR("m"), 0);
		315
		316	uint32_t lon = join_4_bytes(&UBX_buffer[4]);
		317	write_ndigit_number_u (11, 5, (uint16_t)(lon/10000000), 3, 0);
		318	lcd_printp_at (14,5, PSTR("."), 0);
		319	write_ndigit_number_u (15, 5, (uint16_t)((lon/1000) % 10000), 4, 1);
		320	write_ndigit_number_u (19, 5, (uint16_t)((lon/10) % 100), 2, 1);
		321
		322	uint32_t lat = join_4_bytes(&UBX_buffer[8]);
		323	write_ndigit_number_u (11, 6, (uint16_t)(lat/10000000), 3, 0);
		324	lcd_printp_at (14,6, PSTR("."), 0);
		325	write_ndigit_number_u (15, 6, (uint16_t)((lat/1000) % 10000), 4, 1);
		326	write_ndigit_number_u (19, 6, (uint16_t)((lat/10) % 100), 2, 1);
		327
		328	}
		329
		330	}
		331
		332
		333	}
		334
		335	union long_union {
		336	uint32_t dword;
		337	uint8_t byte[4];
		338	} longUnion;
		339
		340	union int_union {
		341	uint16_t dword;
		342	uint8_t byte[2];
		343	} intUnion;
		344
		345	uint32_t join_4_bytes(uint8_t Buffer[])
		346	{
		347	longUnion.byte[0] = *Buffer;
		348	longUnion.byte[1] = *(Buffer+1);
		349	longUnion.byte[2] = *(Buffer+2);
		350	longUnion.byte[3] = *(Buffer+3);
		351	return(longUnion.dword);
		352	}
		353
		354
		355	void checksum(uint8_t data)
		356	{
		357	ck_a += data;
		358	ck_b += ck_a;
		359	}
		360

Subversion Repositories Projects

(root)/Transportables_Koptertool/tags/V3.2/gps.c @ 1126 - Rev