WebSVN - FlightCtrl - Blame - Rev 839 - /branches/KalmanFilter MikeW/Bob4

Rev	Author	Line No.	Line
838	MikeW	1	/*
		2	Copyright 2008, by Michael Walter
		3
		4	All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
		5	General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
		6	WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
		7	See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
		8	License along with this program. If not, see <http://www.gnu.org/licenses/>.
		9
		10	Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
		11	are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
		12	unless it is stated otherwise.
		13	*/
		14
		15	#include "main.h"
		16	#include "kafi.h"
		17
		18	extern void UART2Print(void);
		19	void InitOSD(void);
		20	void SendOSD(void);
		21
		22	extern gpsInfo_t actualPos; // measured position (last gps record)
		23	extern gpsInfo_t targetPos; // measured position (last gps record)
		24
		25	extern signed int GPS_Nick;
		26	extern signed int GPS_Roll;
		27
		28	extern int CurrentAltitude, LastAltitude, targetPosValid, RCQuality;
		29	extern int nick_gain_p, nick_gain_d, roll_gain_p, roll_gain_d;
		30	extern int Override, TargetGier, DeltaAltitude, Theta45, Phi45;
		31	extern f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P;
		32	extern unsigned long maxDistance;
		33
		34	unsigned char UART2PrintAbgeschlossen = 1;
		35	void UART2Print(void);
		36
		37	char DisplayBuff[80] = "";
		38	unsigned char DispPtr = 0;
		39
		40	char OSDBuff[80] = "";
		41	unsigned char OSDPtr = 0;
		42
		43
		44	#define OSD_printf(format, args...) { OSDPtr = 0; _printf_P(OUT_OSD,PSTR(format) , ## args);UART2Print();}
		45	#define OSD_printf_(format, args...) { _printf_P(OUT_OSD,PSTR(format) , ## args);}
		46
		47
		48	/* For OSD_printf to work, Putchar() has to be extendet as follows.
		49	char Putchar(char zeichen)
		50	{
		51	if(PrintZiel == OUT_LCD)
		52	{
		53	DisplayBuff[DispPtr++] = zeichen; return(1);
		54	}
		55	else if (PrintZiel == OUT_OSD)
		56	{
		57	OSDBuff[OSDPtr++] = zeichen; return(1);
		58	}
		59	else
		60	{
		61	return(uart_putchar(zeichen));
		62	}
		63	}
		64	*/
		65
		66
		67	/* ****************************************************************************
		68	Functionname: OsdClear / /!
		69	Description:
		70
		71	@param[in]
		72
		73	@return void
		74	@pre -
		75	@post -
		76	@author Michael Walter
		77	**************************************************************************** */
		78	void OsdClear(void)
		79	{
839	MikeW	80	unsigned char i;
		81	for(i=0;i<80;i++) OSDBuff[i] = ' ';
838	MikeW	82	}
		83
		84	/* ****************************************************************************
		85	Functionname: LcdClear / /!
		86	Description:
		87
		88	@param[in]
		89
		90	@return void
		91	@pre -
		92	@post -
		93	@author
		94	**************************************************************************** */
		95	void LcdClear(void)
		96	{
839	MikeW	97	unsigned char i;
		98	for(i=0;i<80;i++) DisplayBuff[i] = ' ';
838	MikeW	99	}
		100
		101
		102	/* ****************************************************************************
		103	Functionname: InitOSD / /!
		104	Description: Init the Bob-4H OSD
		105
		106	@param[in]
		107
		108	@return void
		109	@pre -
		110	@post -
		111	@author Michael Walter
		112	**************************************************************************** */
		113	void InitOSD()
		114	{
		115	/* Clear OSD */
		116	OSD_printf ("\33[2J");
		117	Delay_ms_Mess(300);
		118	OSD_printf ("\33[2J");
		119
		120	/* Set OSD Pixel Clock */
		121	Delay_ms_Mess(300);
		122	OSD_printf ("\33[23;6v");
		123
		124	/* Set OSD Pixel Width */
		125	Delay_ms_Mess(300);
		126	OSD_printf ("\33[25;448v");
		127
		128	/* Set OSD Font */
		129	Delay_ms_Mess(300);
		130	OSD_printf ("\33[0z");
		131	}
		132
		133
		134
		135	/* ****************************************************************************
		136	Functionname: UART2Print / /!
		137	Description: Send OSD Data to the Bob-4H OSD
		138
		139	@return void
		140	@pre -
		141	@post -
		142	@author Michael Walter
		143	**************************************************************************** */
		144	void UART2Print()
		145	{
		146	OSDBuff[OSDPtr] = '\r';
		147	if (UART2PrintAbgeschlossen == 1)
		148	{
		149	UART2PrintAbgeschlossen = 0;
		150	UDR1 = OSDBuff[0];
		151	}
		152	}
		153
		154	/* ****************************************************************************
		155	Functionname: SIGNAL / /!
		156	Description: Send OSD Data to the Bob-4H OSD
		157
		158	@return void
		159	@pre -
		160	@post -
		161	@author Michael Walter
		162	**************************************************************************** */
		163	SIGNAL(SIG_USART1_TRANS)
		164	{
839	MikeW	165	static unsigned int ptr = 0;
		166
		167	unsigned char tmp_tx;
		168	if(!UART2PrintAbgeschlossen)
		169	{
		170	ptr++; // die [0] wurde schon gesendet
		171	tmp_tx = OSDBuff[ptr];
		172	if((tmp_tx == '\r') /* tmp_tx == 0 */)
		173	{
		174	ptr = 0;
		175	UART2PrintAbgeschlossen = 1;
		176	}
		177	UDR1 = tmp_tx;
		178	}
		179	else ptr = 0;
838	MikeW	180	}
		181
		182
		183	/* ****************************************************************************
		184	Functionname: SendOSD / /!
		185	Description: Display OnScreen Display Data on a Bob-4H
		186
		187	@param[in]
		188
		189	@return void
		190	@pre -
		191	@post -
		192	@author Michael Walter
		193	**************************************************************************** */
		194	void SendOSD()
		195	{
		196	/--- (SYMBOLIC) CONSTANTS ---/
		197
		198	/--- VARIABLES ---/
		199	//static int dx, dy, x1,y1,x2,y2;
		200	//f32_t sinPhi_P_cosTheta_P;
		201	//static int x1_old, y1_old, x2_old, y2_old;
		202
		203	static int iState = 0;
		204
		205	switch (iState)
		206	{
		207	case 0:
		208	OSD_printf ("\33[0;11HHDG:%03d", status.iPsi10 / 10);
		209	iState++;
		210	break;
		211	case 1: /* Display the battery voltage an the RC signal level */
		212	OSD_printf ("\33[0;0HU:%03d\33[1;0HR:%03d", UBat, RCQuality);
		213	iState++;
		214	break;
		215	case 2: /* Display the current altitude */
		216	if (targetPosValid == 1)
		217	{
		218	int DeltaGPSAltitude = (actualPos.altitude - targetPos.altitude);
		219	OSD_printf ("\33[18;20HAGL:%03d\33[17;20HBar:%03d", DeltaGPSAltitude, CurrentAltitude);
		220	}
		221	else
		222	{
		223	OSD_printf ("\33[18;20HAGL:-.-\33[17;20HBar:%03d", CurrentAltitude);
		224	}
		225	iState++;
		226	break;
		227	case 3: /* Draw an artificial horizon. Part 1 */
		228	#if 0
		229	sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P;
		230	Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F));
		231	Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100);
		232	dx = c_cos_8192(Phi45) / 128;
		233	dy = c_sin_8192(Phi45) / 128;
		234	x1 = 180 - dx;
		235	y1 = 120 + 2 * Theta45 + dy;
		236	x2 = 180 + dx;
		237	y2 = y1 - 2 * dy;
		238
		239	OSDPtr = 0;
		240	OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old);
		241	#endif
		242	iState++;
		243	break;
		244	case 4: /* Draw an artificial horizon. Part 2 */
		245	#if 0
		246	OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2);
		247	UART2Print();
		248	x1_old = x1;
		249	y1_old = y1;
		250	x2_old = x2;
		251	y2_old = y2;
		252	#endif
		253	iState++;
		254	break;
		255	case 5: /* Display velocity over ground */
		256	if (actualPos.state > 1)
		257	{
		258	OSD_printf ("\33[0;20HVel:%03d", ((actualPos.groundSpeed / 10) * 36) /100);
		259	}
		260	else
		261	{
		262	OSD_printf ("\33[0;20HVel:-.-");
		263	}
		264	iState++;
		265	break;
		266	case 6: /* Display distance from target */
		267	if (targetPosValid == 1)
		268	{
		269	OSD_printf ("\33[1;20HDst:%03d", maxDistance / 10);
		270	}
		271	else
		272	{
		273	OSD_printf ("\33[1;20HDst:-.-");
		274	}
		275	iState++;
		276	break;
		277	case 7: /* Draw an artificial horizon. Part 1 */
		278	#if 0
		279	sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P;
		280	Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F));
		281	Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100);
		282	dx = c_cos_8192(Phi45) / 128;
		283	dy = c_sin_8192(Phi45) / 128;
839	MikeW	284	x1 = 180 - dx;
838	MikeW	285	y1 = 120 + 2 * Theta45 + dy;
		286	x2 = 180 + dx;
		287	y2 = y1 - 2 * dy;
839	MikeW	288
838	MikeW	289	OSDPtr = 0;
		290	OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old);
		291	#endif
		292	iState++;
		293	break;
		294	case 8: /* Draw an artificial horizon. Part 2 */
		295	#if 0
		296	OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2);
		297	UART2Print();
		298
		299	x1_old = x1;
		300	y1_old = y1;
		301	x2_old = x2;
		302	y2_old = y2;
		303	#endif
		304	iState++;
		305	break;
		306	case 9: /* Display the GPS control outputs */
		307	OSD_printf ("\33[16;0HNG:%03d EG:%03d \33[17;0HNV:%03d EV:%03d ", nick_gain_p, roll_gain_p, nick_gain_d, roll_gain_d);
		308	iState++;
		309	break;
		310	case 10:
		311	OSD_printf ("\33[16;20HClb:%03d", DeltaAltitude);
		312	iState++;
		313	break;
		314	case 11:/* Draw a ^ to indicate the target direction */
		315	{
		316	static int LastGierOffset = 0;
		317	int GierOffset = (TargetGier - status.iPsi10) / 10;
		318	if (GierOffset > 180)
		319	{
		320	GierOffset -= 360;
		321	}
		322	if (GierOffset < -180)
		323	{
		324	GierOffset += 360;
		325	}
		326	GierOffset /= 14;
		327
		328	OSD_printf ("\33[2;%dH \33[2;%dH^", 14 + LastGierOffset,14 + GierOffset);
		329	LastGierOffset = GierOffset;
		330	iState++;
		331	break;
		332	}
		333	case 12: /* Display the GPS_Nick and GPS_Roll / StickNick and StickRoll */
		334	if (Override == 0)
		335	{
		336	OSD_printf ("\33[18;0HGN:%03d GR:%03d ", GPS_Nick, GPS_Roll);
		337	}
		338	else
		339	{
		340	OSD_printf ("\33[18;0HSN:%03d SR:%03d ", StickNick, StickRoll);
		341	}
		342	iState++;
		343	break;
		344	case 13:
		345	#if 0 /* Display the Horizon */
		346	OSD_printf ("\33[156;120.r\33[204;120+r\33[/r ");
		347	#endif
		348	iState=0;
		349	break;
		350	default:
		351	iState = 0;
		352	}
		353	}
		354

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/Bob4_OSD.c - Rev 839