WebSVN - Projects - Blame - /Transportables_Koptertool/PKT/branches/branch

Rev	Author	Line No.	Line
2136	-	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	//# HISTORY mkgpsinfo.c
		36	//#
		37	//# 04.06.2014 OG
		38	//# - chg: MK_Gps_Info() umgestellt auf PKT_Message_Datenverlust()
		39	//#
		40	//# 14.05.2014 OG
		41	//# - chg: Code-Redesign; neues Screen-Layout; Code-Reduzierung;
		42	//# Anpassungen an neue PKT-Funktionen
		43	//# - chg: umbenannt von 'gps.c' zu 'mkgpsinfo.c'
		44	//# - chg: gps() ist jetzt MK_Gps_Info()
		45	//# - add: Source-Historie ergaenzt
		46	//############################################################################
		47
		48
		49	#include <avr/io.h>
		50	#include <inttypes.h>
		51	#include <stdlib.h>
		52	#include <avr/pgmspace.h>
		53	#include <stdbool.h>
		54	#include <util/delay.h>
		55
		56	#include "../main.h"
		57	#include "../lcd/lcd.h"
		58	#include "../timer/timer.h"
		59	#include "../uart/usart.h"
		60	#include "../lipo/lipo.h"
		61	#include "../pkt/pkt.h"
		62	#include "../mk/mkbase.h"
		63	#include "../messages.h"
		64
		65
		66	//##############################################################
		67	//##############################################################
		68
		69
		70	//#define DEBUG_MKGPS // erweiterte Anzeigen zum debuggen
		71
		72	#define GPSTIMEOUT 300 // 3 Sekunden
		73	#define UBX_BUFFER_SIZE 100 // 100 Byte Buffer fuer GPS-Daten
		74
		75
		76	uint8_t ck_a = 0;
		77	uint8_t ck_b = 0;
		78
		79	union long_union
		80	{
		81	uint32_t dword;
		82	uint8_t byte[4];
		83	} longUnion;
		84
		85
		86	union int_union
		87	{
		88	uint16_t dword;
		89	uint8_t byte[2];
		90	} intUnion;
		91
		92
		93	//##############################################################
		94	//##############################################################
		95
		96
		97	//--------------------------------------------------------------
		98	//--------------------------------------------------------------
		99	uint32_t join_4_bytes(uint8_t Buffer[])
		100	{
		101	longUnion.byte[0] = *Buffer;
		102	longUnion.byte[1] = *(Buffer+1);
		103	longUnion.byte[2] = *(Buffer+2);
		104	longUnion.byte[3] = *(Buffer+3);
		105	return (longUnion.dword);
		106	}
		107
		108
		109	//--------------------------------------------------------------
		110	//--------------------------------------------------------------
		111	void checksum(uint8_t data)
		112	{
		113	ck_a += data;
		114	ck_b += ck_a;
		115	}
		116
		117
		118
		119	//--------------------------------------------------------------
		120	//--------------------------------------------------------------
		121	void MK_Gps_Info( void )
		122	{
		123	uint8_t UBX_buffer[UBX_BUFFER_SIZE];
		124	uint8_t UBX_payload_counter = 0;
		125	uint8_t UBX_class = 0;
		126	uint8_t UBX_id = 0;
		127	uint8_t UBX_ck_a = 0;
		128	uint8_t data = 0;
		129	uint8_t length = 0;
		130	uint8_t state = 0;
		131
		132	uint8_t redraw = true;
		133	uint8_t refresh = false;
		134	uint8_t yoffs = 1; // Anzeige-Verschiebung: der obere Bereich
		135	uint8_t yoffs2 = 4; // Anzeige-Verschiebung: GPS-Koordinaten
		136	int16_t v_16; // tmp-Variable
		137	int32_t v_32; // tmp-Variable
		138
		139	#ifdef DEBUG_MKGPS
		140	uint8_t maxlen = 0;
		141	#endif
		142
		143	MK_SwitchToNC(); // Anmerkung OG: warum auch immer es besser ist erst auf die NC
		144	MK_SwitchToGPS(); // umzuschalten... es laeuft so scheinbar zuverlaessiger
		145	MK_SwitchToGPS();
		146
		147	timer_mk_timeout = GPSTIMEOUT;
		148
		149	do
		150	{
		151	//------------------------
		152	// REDRAW
		153	//------------------------
		154	if( redraw )
		155	{
		156	ShowTitle_P( PSTR("MK GPS Info"), true);
		157
		158	lcdx_printp_at( 0,1, PSTR("Fix:"), MNORMAL, 0,yoffs );
		159	lcdx_printp_at( 0,2, PSTR("Sat:"), MNORMAL, 0,yoffs );
		160	lcdx_printp_at( 0,3, PSTR("Alt:"), MNORMAL, 0,yoffs );
		161
		162	lcdx_printp_at( 9,1, PSTR("PDOP:"), MNORMAL, 3,yoffs );
		163	lcdx_printp_at( 9,2, PSTR("Accu:"), MNORMAL, 3,yoffs );
		164	lcdx_printp_at( 9,3, PSTR("Sped:"), MNORMAL, 3,yoffs );
		165
		166	lcd_frect( 0, (4*7)+4+yoffs2, 127, 7, 1); // GPS: Rect: Invers
		167	lcdx_printp_at(1, 3, strGet(START_LASTPOS1), MINVERS, 0,8+yoffs2); // GPS: "Breitengr L�ngengr"
		168	lcd_rect( 0, (38)+7+yoffs2, 127, (28)+4, 1); // GPS: Rahmen
		169
		170	lcdx_printp_at(12,7, strGet(ENDE), MNORMAL, 0,1); // KEYLINE
		171
		172	redraw = false;
		173	}
		174
		175
		176	//------------------------
		177	// REFRESH
		178	//------------------------
		179	if( refresh )
		180	{
		181	if((UBX_class == 1) && (UBX_id == 6)) // GPS: SVINFO
		182	{
		183	//--------------
		184	// GPS Status
		185	//--------------
		186	switch( UBX_buffer[10] )
		187	{
		188	case 4:
		189	case 3: lcdx_printp_at( 5, 1, PSTR("3D"), MNORMAL, 1,yoffs); break;
		190	case 2: lcdx_printp_at( 5, 1, PSTR("2D"), MNORMAL, 1,yoffs); break;
		191	default: lcdx_printp_at( 5, 1, PSTR("no"), MNORMAL, 1,yoffs);
		192	}
		193
		194	// GPS ok => ein Haken wird angezeigt
		195	if( (UBX_buffer[11] & 1) == 1 ) lcdx_putc( 7, 1, SYMBOL_CHECK, MNORMAL, 3,yoffs );
		196	else lcdx_putc( 7, 1, ' ', MNORMAL, 3,yoffs );
		197
		198	//---
		199	// Anzeige von "D" - evtl. DGPS (Differential GPS)?
		200	// aktuell nicht mehr unterstuetzt da kein Platz auf dem Screen
		201	//---
		202	//if( (UBX_buffer[11] & 3) == 3 ) lcd_printp_at (10,0, PSTR("D"), 0);
		203	//else lcd_printp_at (10,0, PSTR(" "), 0);
		204
		205	//--------------
		206	// Sat
		207	//--------------
		208	lcdx_printf_at_P( 5, 2, MNORMAL, 0,yoffs, PSTR("%2u"), UBX_buffer[47] );
		209
		210	//--------------
		211	// PDOP
		212	//--------------
		213	v_16 = UBX_buffer[44]+UBX_buffer[45]*255;
		214	lcdx_printf_at_P( 15, 1, MNORMAL, 0,yoffs, PSTR("%2.2d"), v_16 );
		215
		216	//--------------
		217	// Accuracy
		218	//--------------
		219	v_32 = (int32_t)join_4_bytes(&UBX_buffer[24]);
		220	lcdx_printf_at_P( 15, 2, MNORMAL, 0,yoffs, PSTR("%2.2ldm"), v_32 );
		221	}
		222
		223	if((UBX_class == 1) && (UBX_id == 18)) // GPS: VELNED
		224	{
		225	//--------------
		226	// Speed
		227	//--------------
		228	v_16 = (int16_t)((join_4_bytes(&UBX_buffer[20])6060)/100000);
		229	lcdx_printf_at_P( 15, 3, MNORMAL, 0,yoffs, PSTR("%3dkmH"), v_16 );
		230
		231	//uint16_t speed = (uint16_t)((join_4_bytes(&UBX_buffer[20])6060)/100000);
		232	//write_ndigit_number_u (11, 4, speed, 3, 0,0);
		233	//lcd_printp_at (15,4, PSTR("km/h"), 0);
		234	}
		235
		236	if((UBX_class == 1) && (UBX_id == 2)) // GPS: POSLLH
		237	{
		238	//--------------
		239	// Altitude
		240	//--------------
		241	v_16 = (int16_t)(join_4_bytes(&UBX_buffer[16])/1000);
		242	//v_16 = v_16 + 3000;
		243	lcdx_printf_at_P( 4, 3, MNORMAL, 1,yoffs, PSTR("%4d"), v_16 );
		244
		245	//uint16_t height = (uint16_t)(join_4_bytes(&UBX_buffer[16])/1000);
		246	//write_ndigit_number_u (11, 7, height, 4, 0,0);
		247	//lcd_printp_at(16,7, PSTR("m"), 0);
		248
		249
		250	//--------------
		251	// Breitengrad - Lat (51.)
		252	//--------------
		253	v_32 = join_4_bytes(&UBX_buffer[8]);
		254	writex_gpspos( 1, 4, v_32 , MNORMAL, 0,10+yoffs2); // Anzeige: Breitengrad
		255
		256	//write_ndigit_number_u ( 0, 7, (uint16_t)(lat/10000000), 2, 0,0);
		257	//lcd_printp_at ( 2, 7, PSTR("."), 0);
		258	//write_ndigit_number_u ( 3, 7, (uint16_t)((lat/1000) % 10000), 4, 1,0);
		259	//write_ndigit_number_u ( 7, 7, (uint16_t)((lat/10) % 100), 2, 1,0);
		260
		261
		262	//--------------
		263	// Laengengrad - Long (7.)
		264	//--------------
		265	v_32 = join_4_bytes(&UBX_buffer[4]);
		266	writex_gpspos( 12, 4, v_32, MNORMAL, -1,10+yoffs2); // Anzeige: Laengengrad
		267
		268	//write_ndigit_number_u (10, 7, (uint16_t)(lon/10000000), 2, 0,0);
		269	//lcd_printp_at (12, 7, PSTR("."), 0);
		270	//write_ndigit_number_u (13, 7, (uint16_t)((lon/1000) % 10000), 4, 1,0);
		271	//write_ndigit_number_u (17, 7, (uint16_t)((lon/10) % 100), 2, 1,0);
		272	}
		273
		274	//------------------------
		275	// PKT-LiPo Anzeige
		276	//------------------------
		277	show_Lipo();
		278
		279	refresh = false;
		280	} // end: if( refresh )
		281
		282
		283	//--------------------------
		284	// PROCESS DATA
		285	//--------------------------
		286	if( uart_getc_nb( &data ) )
		287	{
		288	switch( state )
		289	{
		290	case 0: if( data == 0xB5 ) // GPS: init 1
		291	{
		292	UBX_payload_counter = 0;
		293	UBX_id = 0;
		294	UBX_class = 0;
		295	ck_a = 0;
		296	ck_b = 0;
		297	state++;
		298	}
		299	break;
		300
		301
		302	case 1: if( data == 0x62 ) // GPS: init 2
		303	state++;
		304	else
		305	state = 0;
		306	break;
		307
		308
		309	case 2: if( data != 1 ) // GPS: class
		310	state = 0;
		311	else
		312	{
		313	checksum(data);
		314	UBX_class = data;
		315	state++;
		316	}
		317	break;
		318
		319
		320	case 3: if( (data != 48) // GPS: id
		321	&&(data != 6 )
		322	&&(data != 18)
		323	&&(data != 2 ))
		324	{
		325	state = 0;
		326	}
		327	else
		328	{
		329	UBX_id = data;
		330	checksum(data);
		331	state++;
		332	}
		333	break;
		334
		335
		336	case 4: if( data >= UBX_BUFFER_SIZE ) // GPS: length lo
		337	state = 0;
		338	else
		339	{
		340	checksum(data);
		341	length = data;
		342	state++;
		343	}
		344
		345	#ifdef DEBUG_MKGPS
		346	// DEBUG - Anzeige der Paketgroesse
		347	if( data > maxlen )
		348	{
		349	// groesstes gemessenes Paket lag bei 181
		350	// ob man das Paket braucht ist eine andere Frage
		351	maxlen = data;
		352	lcdx_printf_at_P( 1, 7, MNORMAL, 0,1, PSTR("%3u"), maxlen );
		353	set_beep( 25, 0xffff, BeepNormal ); // kurzer Beep
		354	}
		355	#endif
		356	break;
		357
		358
		359	case 5: if( data != 0 ) // GPS: length hi
		360	state = 0;
		361	else
		362	{
		363	checksum(data);
		364	state++;
		365	}
		366	break;
		367
		368
		369	case 6: UBX_buffer[UBX_payload_counter] = data; // GPS: data
		370	checksum(data);
		371	UBX_payload_counter++;
		372	length--;
		373	if( length == 0 )
		374	{
		375	state++;
		376	};
		377	break;
		378
		379
		380	case 7: state++; // GPS: check lo
		381	UBX_ck_a = data;
		382	break;
		383
		384
		385	case 8: state = 0; // GPS: check hi
		386	if((UBX_ck_a == ck_a) && (data == ck_b))
		387	{
		388	timer_mk_timeout = GPSTIMEOUT;
		389	refresh = true;
		390	}
		391
		392	} // end: switch( state )
		393	} // end: if( uart_getc_nb(&data) )
		394
		395
		396	//------------------------------------------
		397	// Pruefe PKT-Update oder andere PKT-Aktion
		398	//------------------------------------------
		399	/*
		400	// funktioniert hier nicht - warum auch immer
		401	if( PKT_CtrlHook() ) // Update vom Updatetool angefordert?
		402	{
		403	redraw = true;
		404	}
		405	*/
		406
		407
		408	} while( !get_key_press(1 << KEY_ESC) && timer_mk_timeout );
		409
		410
		411
		412	//--------------------------
		413	// in den Timeout gelaufen?
		414	//--------------------------
		415	if( !timer_mk_timeout )
		416	{
		417	//PKT_Message_Datenverlust( timeout, beep)
		418	PKT_Message_Datenverlust( 500, true); // 500 = 5 Sekunden
		419	}
		420
		421	clear_key_all();
		422
		423	SwitchToNC();
		424	}
		425

Subversion Repositories Projects

(root)/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/mk/mkgpsinfo.c @ 2208 - Rev