WebSVN - Projects - Blame - Rev 1470 - /Transportables_Koptertool/branch/GPL_PKT_V3_5_8a

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

Subversion Repositories Projects

(root)/Transportables_Koptertool/branch/GPL_PKT_V3_5_8a_FC086/gps.c @ 1497 - Rev 1470