WebSVN - Projects - Blame - Rev 724 - /Transportables

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

Subversion Repositories Projects

(root)/Transportables_Koptertool/trunk/V-0.1.2/osd.c - Rev 724