WebSVN - Projects - Blame - Rev 730 - /Transportables

Rev	Author	Line No.	Line
730	woggle	1	/*****************************************************************************
		2	* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net *
		3	* based on the C-OSD code from CaScAdE *
		4	* http://www.mylifesucks.de/oss/c-osd/ *
		5	* *
		6	* This program is free software; you can redistribute it and/or modify *
		7	* it under the terms of the GNU General Public License as published by *
		8	* the Free Software Foundation; either version 2 of the License. *
		9	* *
		10	* This program is distributed in the hope that it will be useful, *
		11	* but WITHOUT ANY WARRANTY; without even the implied warranty of *
		12	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
		13	* GNU General Public License for more details. *
		14	* *
		15	* You should have received a copy of the GNU General Public License *
		16	* along with this program; if not, write to the *
		17	* Free Software Foundation, Inc., *
		18	* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
		19	* *
		20	*****************************************************************************/
		21
		22	#include <avr/io.h>
		23	#include <inttypes.h>
		24	#include <stdlib.h>
		25	#include <avr/pgmspace.h>
		26
		27	#include "main.h"
		28	#include "osd.h"
		29	#include "lcd.h"
		30	#include "timer.h"
		31	#include "usart.h"
		32
		33	#include "mk-data-structs.h"
		34
		35	#define COSD_WASFLYING 4
		36
		37	/* ##########################################################################
		38	* global definitions and global vars
		39	* ##########################################################################*/
		40	NaviData_t *naviData;
		41
		42	// stats for after flight
		43	int16_t max_Altimeter = 0;
		44	uint16_t max_GroundSpeed = 0;
		45	int16_t max_Distance = 0;
		46	uint8_t min_UBat = 255;
		47	uint16_t max_FlyingTime = 0;
		48	uint16_t max_Current = 0;
		49	uint16_t max_Capacity = 0;
		50
		51	// cache old vars for blinking attribute, checkup is faster than full
		52	// attribute write each time
		53	volatile uint8_t last_UBat = 255;
		54	volatile uint8_t last_RC_Quality = 255;
		55
		56	volatile uint16_t ftimer = 0;
		57
		58	// store stats description in progmem to save space
		59	const char stats_item_0[] PROGMEM = "max Altitude:";
		60	const char stats_item_1[] PROGMEM = "max Speed :";
		61	const char stats_item_2[] PROGMEM = "max Distance:";
		62	const char stats_item_3[] PROGMEM = "min Voltage :";
		63	const char stats_item_4[] PROGMEM = "max Time :";
		64	#if 1
		65	const char stats_item_5[] PROGMEM = "max Current :";
		66	const char stats_item_6[] PROGMEM = "UsedCapacity:";
		67	#else
		68	const char stats_item_5[] PROGMEM = "Long. :";
		69	const char stats_item_6[] PROGMEM = "Lat. :";
		70	#endif
		71	const char *stats_item_pointers[] PROGMEM = {
		72	stats_item_0,
		73	stats_item_1,
		74	stats_item_2,
		75	stats_item_3,
		76	stats_item_4,
		77	stats_item_5,
		78	stats_item_6
		79	};
		80
		81	//char* rose = "-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W";
		82	const char rose[48] PROGMEM = {
		83	0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S',
		84	0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O',
		85	0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N',
		86	0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W'};
		87	// the center is char 19 (north), we add the current heading in 8th
		88	// which would be 22.5 degrees, but float would bloat up the code
		89	// and *10 / 225 would take ages... so we take the uncorrect way
		90
		91
		92	const char str_NE[] PROGMEM = "NE";
		93	const char str_E[] PROGMEM = "E ";
		94	const char str_SE[] PROGMEM = "SE";
		95	const char str_S[] PROGMEM = "S ";
		96	const char str_SW[] PROGMEM = "SW";
		97	const char str_W[] PROGMEM = "W ";
		98	const char str_NW[] PROGMEM = "NW";
		99	const char str_N[] PROGMEM = "N ";
		100	const char *directions_p[8] PROGMEM = {
		101	str_NE,
		102	str_E,
		103	str_SE,
		104	str_S,
		105	str_SW,
		106	str_W,
		107	str_NW,
		108	str_N
		109	};
		110
		111	// Flags
		112	uint8_t COSD_FLAGS2 = 0;
		113
		114
		115	/**
		116	* convert the <heading> gotton from NC into an index
		117	*/
		118	uint8_t heading_conv (uint16_t heading)
		119	{
		120	if (heading > 23 && heading < 68)
		121	{
		122	return 0; //direction = "NE";
		123	}
		124	else if (heading > 67 && heading < 113)
		125	{
		126	return 1; //direction = "E ";
		127	}
		128	else if (heading > 112 && heading < 158)
		129	{
		130	return 2; //direction = "SE";
		131	}
		132	else if (heading > 157 && heading < 203)
		133	{
		134	return 3; //direction = "S ";
		135	}
		136	else if (heading > 202 && heading < 248)
		137	{
		138	return 4; //direction = "SW";
		139	}
		140	else if (heading > 247 && heading < 293)
		141	{
		142	return 5; //direction = "W ";
		143	}
		144	else if (heading > 292 && heading < 338)
		145	{
		146	return 6; //direction = "NW";
		147	}
		148	return 7; //direction = "N ";
		149	}
		150
		151	/**
		152	* draw a compass rose at <x>/<y> for <heading>
		153	*/
		154	void draw_compass (uint8_t x, uint8_t y, uint16_t heading)
		155	{
		156	uint8_t front = 19 + (heading / 22);
		157	for (uint8_t i = 0; i < 9; i++)
		158	{
		159	lcd_putc (x++, y, pgm_read_byte(&rose[front - 4 + i]), 0);
		160	}
		161	}
		162
		163	/* ##########################################################################
		164	* variometer
		165	* ##########################################################################*/
		166	/**
		167	* draw variometer arrows at <x>/<y> according to <variometer>
		168	*/
		169	void draw_variometer (uint8_t x, uint8_t y, uint8_t width_x, uint8_t width_y, int16_t variometer)
		170	{
		171	lcd_rect (x, y - ((width_y - 1) / 2), width_x, width_y, 1);
		172	lcd_frect (x + 1, y - ((width_y - 1) / 2) + 1, width_x - 2, width_y - 2, 0);
		173	lcd_line (x, y, x + width_x, y, 1);
		174
		175	if (variometer > 0)
		176	{ // gain height
		177	switch (variometer / 5)
		178	{
		179	case 0:
		180	lcd_frect (x + 3, y - 1, 3, 1, 1);
		181	break;
		182	case 1:
		183	lcd_frect (x + 2, y - 3, 5, 3, 1);
		184	break;
		185	case 2:
		186	lcd_frect (x + 2, y - 4, 5, 4, 1);
		187	break;
		188	default:
		189	lcd_frect (x + 1, y - 5, 7, 5, 1);
		190	break;
		191	}
		192	}
		193	else
		194	{ // sink
		195	switch (variometer / -5)
		196	{
		197	case 0:
		198	lcd_frect (x + 3, y, 3, 1, 1);
		199	break;
		200	case 1:
		201	lcd_frect (x + 2, y, 5, 3, 1);
		202	break;
		203	case 2:
		204	lcd_frect (x + 2, y, 5, 4, 1);
		205	break;
		206	default:
		207	lcd_frect (x + 1, y, 7, 5, 1);
		208	break;
		209	}
		210	}
		211	}
		212
		213
		214	#define TIMEOUT 200 // 2 sec
		215
		216	void print_statistics (void)
		217	{
		218	uint8_t line = 0;
		219	lcd_cls ();
		220
		221	// max Altitude
		222	lcd_printpns_at (0, line, stats_item_pointers[0], 0);
		223	write_ndigit_number_s (13, line, max_Altimeter / 30, 4, 0);
		224	lcd_putc (17, line, 'm', 0);
		225
		226	// max Speed
		227	lcd_printpns_at (0, ++line, stats_item_pointers[1], 0);
		228	write_ndigit_number_u (14, line, (uint16_t) (((uint32_t) max_GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0);
		229	lcd_printpns_at(17, line, PSTR("km/h"), 0);
		230
		231	// max Distance
		232	lcd_printpns_at (0, ++line, stats_item_pointers[2], 0);
		233	write_ndigit_number_u (14, line, max_Distance / 10, 3, 0);
		234	lcd_putc (17, line, 'm', 0);
		235
		236	// max time
		237	lcd_printpns_at (0, ++line, stats_item_pointers[4], 0);
		238	write_time (13, line, max_FlyingTime);
		239
		240	// min voltage
		241	lcd_printpns_at (0, ++line, stats_item_pointers[3], 0);
		242	write_ndigit_number_u_10th (13, line, min_UBat, 3, 0);
		243	lcd_putc (17, line, 'V', 0);
		244
		245	#if 1
		246	// max Current
		247	lcd_printpns_at (0, ++line, stats_item_pointers[5], 0);
		248	write_ndigit_number_u_10th (13, line, max_Current, 3, 0);
		249	lcd_putc (17, line, 'A', 0);
		250
		251	// Used Capacity
		252	lcd_printpns_at (0, ++line, stats_item_pointers[6], 0);
		253	write_ndigit_number_u (13, line, max_Capacity, 4, 0);
		254	lcd_printpns_at(17, line, PSTR("mAh"), 0);
		255	#else
		256	// longitude
		257	lcd_printpns_at (0, ++line, stats_item_pointers[5], 0);
		258	write_gps_pos (8, line, naviData->CurrentPosition.Longitude);
		259
		260	// latitude
		261	lcd_printpns_at (0, ++line, stats_item_pointers[6], 0);
		262	write_gps_pos (8, line, naviData->CurrentPosition.Latitude);
		263	#endif
		264	while (!get_key_press (1 << KEY_ESC))
		265	timer = TIMEOUT;
		266	COSD_FLAGS2 &= ~COSD_WASFLYING;
		267	get_key_press(KEY_ALL);
		268	lcd_cls();
		269	}
		270
		271	void osd (void)
		272	{
		273	uint8_t flag;
		274	uint8_t tmp_dat;
		275
		276	// Clear statistics
		277	max_Altimeter = 0;
		278	max_GroundSpeed = 0;
		279	max_Distance = 0;
		280	min_UBat = 255;
		281	max_FlyingTime = 0;
		282
		283	// flags from last round to check for changes
		284	uint8_t old_FCFlags = 0;
		285
		286	uint16_t old_hh = 0;
		287
		288	lcd_cls();
		289
		290	if (hardware == FC)
		291	{
		292	lcd_printp_at(0, 3, PSTR("Only with NC !"), 0);
		293	timer = 100;
		294	while (timer > 0);
		295	}
		296
		297	SwitchToNC();
		298
		299	mode = 'O';
		300
		301	// disable debug...
		302	// RS232_request_mk_data (0, 'd', 0);
		303	tmp_dat = 0;
		304	SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);
		305
		306	// request OSD Data from NC every 100ms
		307	// RS232_request_mk_data (1, 'o', 100);
		308	tmp_dat = 10;
		309	SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);
		310
		311	flag = 0;
		312	timer = TIMEOUT;
		313	abo_timer = ABO_TIMEOUT;
		314
		315	do
		316	{
		317	if (rxd_buffer_locked)
		318	{
		319	timer = TIMEOUT;
		320	Decode64 ();
		321	naviData = (NaviData_t *) pRxData;
		322
		323	flag = 1;
		324
		325	if (naviData->FCFlags & FCFLAG_MOTOR_RUN)
		326	{ // should be engines running
		327	// motors are on, assume we were/are flying
		328	COSD_FLAGS2 \|= COSD_WASFLYING;
		329	}
		330	else
		331	{ // stats
		332	if ((COSD_FLAGS2 & COSD_WASFLYING) \|\| (get_key_press (1 << KEY_ENTER)))
		333	{
		334	print_statistics ();
		335	}
		336	}
		337
		338	// lcd_printpns_at (0, 3, PSTR("012345678901234567890"), 0);
		339	lcd_ecircle(22, 35, 16, 1);
		340
		341	// Ground Speed
		342	write_ndigit_number_u (1, 0, (uint16_t) (((uint32_t) naviData->GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0);
		343	lcd_printpns_at(4, 0, PSTR("km/h"), 0);
		344
		345	// Compass
		346	write_ndigit_number_u (14, 0, naviData->CompassHeading, 3, 0);
		347	lcd_putc (17, 0, 0x1E, 0); // degree symbol
		348	lcd_printpns_at (18, 0, (const char *) (pgm_read_word ( &(directions_p[heading_conv(naviData->CompassHeading)]))), 0);
		349
		350	draw_compass (12, 1, naviData->CompassHeading);
		351
		352	// Altitude
		353	//note:lephisto:according to several sources it's /30
		354	if (naviData->Altimeter > 300 \|\| naviData->Altimeter < -300)
		355	{
		356	// above 10m only write full meters
		357	write_ndigit_number_s (0, 1, naviData->Altimeter / 30, 4, 0);
		358	}
		359	else
		360	{
		361	// up to 10m write meters.dm
		362	write_ndigit_number_s_10th (0, 1, naviData->Altimeter / 3, 3, 0);
		363	}
		364	lcd_putc (4, 1, 'm', 0);
		365
		366	draw_variometer (55, 7, 9, 13, naviData->Variometer);
		367
		368	// TODO: verify correctness
		369	uint16_t heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360;
		370	lcd_ecirc_line (22, 35, 15, old_hh, 0);
		371	old_hh = heading_home;
		372	lcd_ecirc_line (22, 35, 15, heading_home, 1);
		373
		374	write_ndigit_number_u (7, 3, heading_home, 3, 0);
		375	lcd_putc (10, 3, 0x1e, 0); // degree symbol
		376
		377	write_ndigit_number_u (7, 2, naviData->HomePositionDeviation.Distance / 10, 3, 0);
		378	lcd_putc (10, 2, 'm', 0);
		379
		380	// Sats in use
		381	lcd_printp_at(10, 4, PSTR("Sats"), 0);
		382	write_ndigit_number_u (8, 4, naviData->SatsInUse, 2, 0);
		383
		384	if (naviData->NCFlags & NC_FLAG_MANUAL_CONTROL)
		385	{
		386	lcd_putc (19, 4, 'M', 0); // rc transmitter
		387	}
		388	else
		389	{
		390	lcd_putc (19, 4, ' ', 0); // clear
		391	}
		392	#if 0
		393	lcd_printp_at(11, 5, PSTR("Mode:"), 0);
		394	if (naviData->NCFlags & NC_FLAG_CH)
		395	{
		396	lcd_printpns_at (17, 5, PSTR("CH "), 0);
		397	}
		398	else if (naviData->NCFlags & NC_FLAG_PH)
		399	{
		400	lcd_printpns_at (17, 5, PSTR("PH "), 0);
		401	}
		402	else
		403	{ // (naviData->NCFlags & NC_FLAG_FREE)
		404	lcd_printpns_at (17, 5, PSTR("Free"), 0); // sat2 (free)
		405	}
		406	#endif
		407	if (naviData->NCFlags & NC_FLAG_CH)
		408	{
		409	lcd_printpns_at (10, 5, PSTR("Coming Home"), 0);
		410	}
		411	else if (naviData->NCFlags & NC_FLAG_PH)
		412	{
		413	lcd_printpns_at (10, 5, PSTR("Pos. Hold "), 0);
		414	}
		415	else
		416	{ // (naviData->NCFlags & NC_FLAG_FREE)
		417	lcd_printpns_at (10, 5, PSTR("Free "), 0);
		418	}
		419
		420	// Flying time
		421	write_time (7, 6, naviData->FlyingTime);
		422	// lcd_printp_at (7, 6, PSTR("Fly"), 0);
		423
		424	// RC
		425	write_ndigit_number_u (15, 6, naviData->RC_Quality, 3, 0);
		426	lcd_putc (18, 6, 0x1F, 0); // RC-transmitter
		427	if (naviData->NCFlags & NC_FLAG_NOSERIALLINK)
		428	{
		429	lcd_printpns_at(19, 6, PSTR(" "), 0); // clear
		430	}
		431	else
		432	{
		433	lcd_printpns_at(19, 6, PSTR("PC"), 0);
		434	}
		435
		436	// Battery level
		437	write_ndigit_number_u_10th (0, 7, naviData->UBat, 3, 0);
		438	lcd_putc (4, 7, 'V', 0);
		439
		440	// Current
		441	write_ndigit_number_u_10th (7, 7, naviData->Current, 3, 0);
		442	lcd_putc (11, 7, 'A', 0);
		443
		444	// Capacity
		445	write_ndigit_number_u (14, 7, naviData->UsedCapacity, 4, 0);
		446	lcd_printpns_at(18, 7, PSTR("mAh"), 0);
		447
		448	// remember statistics (only when engines running)
		449	if (naviData->FCFlags & FCFLAG_MOTOR_RUN)
		450	{
		451	if (naviData->Altimeter > max_Altimeter) max_Altimeter = naviData->Altimeter;
		452	if (naviData->GroundSpeed > max_GroundSpeed) max_GroundSpeed = naviData->GroundSpeed;
		453	if (naviData->HomePositionDeviation.Distance > max_Distance) max_Distance = naviData->HomePositionDeviation.Distance;
		454	if (naviData->UBat < min_UBat) min_UBat = naviData->UBat;
		455	if (naviData->FlyingTime > max_FlyingTime) max_FlyingTime = naviData->FlyingTime;
		456	if (naviData->Current > max_Current) max_Current = naviData->Current;
		457	if (naviData->UsedCapacity > max_Capacity) max_Capacity = naviData->UsedCapacity;
		458	}
		459
		460	// remember last values
		461	last_RC_Quality = naviData->RC_Quality;
		462	last_UBat = naviData->UBat;
		463	old_FCFlags = naviData->FCFlags;
		464
		465	rxd_buffer_locked = FALSE;
		466	}
		467
		468	if (!abo_timer)
		469	{ // renew abo every 3 sec
		470	// request OSD Data from NC every 100ms
		471	// RS232_request_mk_data (1, 'o', 100);
		472	tmp_dat = 10;
		473	SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);
		474
		475	abo_timer = ABO_TIMEOUT;
		476	}
		477	}
		478	while (!get_key_press (1 << KEY_ESC) && timer);
		479	get_key_press(KEY_ALL);
		480
		481	// disable OSD Data from NC
		482	// RS232_request_mk_data (1, 'o', 0);
		483	tmp_dat = 0;
		484	SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);
		485
		486	mode = 0;
		487	rxd_buffer_locked = FALSE;
		488
		489	if (!timer)
		490	{ // timeout occured
		491	if (flag)
		492	{
		493	lcd_cls ();
		494	}
		495	lcd_printp_at (0, 2, PSTR("ERROR: no data"), 0);
		496
		497	timer = 100;
		498	while (timer > 0);
		499	print_statistics ();
		500	}
		501	}

Subversion Repositories Projects

(root)/Transportables_Koptertool/trunk/osd.c @ 2619 - Rev 730