WebSVN - NaviCtrl - Blame - Rev 171

Rev	Author	Line No.	Line
41	ingob	1	/#######################################################################################/
		2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
		3	/#######################################################################################/
		4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		5	// + Copyright (c) 2008 Ingo Busker, Holger Buss
171	ingob	6	// + Nur f�r den privaten Gebrauch / NON-COMMERCIAL USE ONLY
41	ingob	7	// + FOR NON COMMERCIAL USE ONLY
		8	// + www.MikroKopter.com
		9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		10	// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
		11	// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist.
		12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
		13	// + bzgl. der Nutzungsbedingungen aufzunehmen.
		14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
		15	// + Verkauf von Luftbildaufnahmen, usw.
		16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
		18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
		19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
		21	// + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
		22	// + eindeutig als Ursprung verlinkt werden
		23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		24	// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
		25	// + Benutzung auf eigene Gefahr
		26	// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
		27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
171	ingob	28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
41	ingob	29	// + mit unserer Zustimmung zul�ssig
		30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		31	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
		32	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		33	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
		34	// + this list of conditions and the following disclaimer.
		35	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
		36	// + from this software without specific prior written permission.
		37	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
		38	// + for non-commercial use (directly or indirectly)
		39	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
		40	// + with our written permission
		41	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
		42	// + clearly linked as origin
171	ingob	43	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
41	ingob	44	//
		45	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
		46	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
		47	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
		48	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
		49	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
		50	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
		51	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
		52	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
		53	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
		54	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
		55	// + POSSIBILITY OF SUCH DAMAGE.
		56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		57	#include <stdio.h>
		58	#include <stdarg.h>
		59	#include <string.h>
		60
		61	#include "91x_lib.h"
		62	#include "ramfunc.h"
		63	#include "menu.h"
		64	#include "printf_P.h"
		65	#include "GPS.h"
		66	#include "i2c.h"
		67	#include "uart0.h"
		68	#include "uart1.h"
		69	#include "uart2.h"
		70	#include "timer.h"
		71	#include "usb.h"
		72	#include "main.h"
		73	#include "waypoints.h"
		74	#include "GPS.h"
		75
		76	// slave addresses
		77	#define FC_ADDRESS 1
		78	#define NC_ADDRESS 2
		79	#define MK3MAG_ADDRESS 3
		80
		81	#define FALSE 0
		82	#define TRUE 1
		83
92	killagreg	84	typedef struct
		85	{
		86	u8 SWMajor;
		87	u8 SWMinor;
		88	u8 ProtoMajor;
		89	u8 ProtoMinor;
		90	u8 SWPatch;
		91	u8 Reserved[5];
		92	} __attribute__((packed)) UART_VersionInfo_t;
		93
		94
41	ingob	95	u8 Request_SendFollowMe = FALSE;
		96	u8 Request_VerInfo = FALSE;
		97	u8 Request_ExternalControl = FALSE;
		98	u8 Request_Display = FALSE;
		99	u8 Request_Display1 = FALSE;
		100	u8 Request_DebugData = FALSE;
		101	u8 Request_DebugLabel = 255;
		102	u8 Request_ChannelOnly = FALSE;
		103	u8 Request_NaviData = FALSE;
		104	u8 Request_ErrorMessage = FALSE;
		105	u8 Request_NewWaypoint = FALSE;
92	killagreg	106	u8 Request_ReadWaypoint = 255;
		107	u8 Request_Data3D = FALSE;
		108	u8 Request_Echo = FALSE;
41	ingob	109
		110	u8 DisplayLine = 0;
		111
		112	UART_TypeDef *DebugUART = UART1;
		113
		114	volatile u8 txd_buffer[TXD_BUFFER_LEN];
		115	volatile u8 rxd_buffer_locked = FALSE;
		116	volatile u8 rxd_buffer[RXD_BUFFER_LEN];
		117	volatile u8 txd_complete = TRUE;
		118	volatile u8 ReceivedBytes = 0;
		119	volatile u8 CntCrcError = 0;
		120	volatile u8 *pRxData = NULL;
92	killagreg	121	volatile u8 RxDataLen = 0;
		122	volatile u8 SerialLinkOkay = 0;
41	ingob	123
		124	u8 text[100];
		125
		126	u8 PcAccess = 100;
		127	u8 MotorTest[4] = {0,0,0,0};
		128	u8 ConfirmFrame = 0;
		129
		130	DebugOut_t DebugOut;
		131	ExternControl_t ExternControl;
		132	UART_VersionInfo_t UART_VersionInfo;
		133	NaviData_t NaviData;
		134	Waypoint_t FollowMe;
61	holgerb	135	Data3D_t Data3D;
92	killagreg	136	u16 Echo; // 2 bytes recieved will be sent back as echo
41	ingob	137
		138	u32 DebugData_Timer;
75	holgerb	139	u32 DebugData_Interval = 5000; // in ms
41	ingob	140	u32 NaviData_Timer;
75	holgerb	141	u32 NaviData_Interval = 5000; // in ms
62	killagreg	142	u32 Data3D_Timer = 0; // in ms
		143	u32 Data3D_Interval = 0;
41	ingob	144
		145	static u16 ptr_txd_buffer = 0;
		146
		147	const u8 ANALOG_LABEL[32][16] =
		148	{
90	killagreg	149	//1234567890123456
41	ingob	150	"AngleNick ", //0
		151	"AngleRoll ",
		152	"AccNick ",
		153	"AccRoll ",
		154	" ",
75	holgerb	155	"MK-Flags ", //5
95	killagreg	156	"NC-Flags ",
41	ingob	157	" ",
		158	" ",
		159	"GPS Data ",
61	holgerb	160	"CompassHeading ", //10
		161	"GyroHeading ",
41	ingob	162	"SPI Error ",
		163	"SPI Okay ",
		164	"I2C Error ",
		165	"I2C Okay ", //15
		166	" ",// "FC_Kalman_K ",
		167	"ACC_Speed_N ",
		168	"ACC_Speed_E ",
		169	" ",// "GPS ACC ",
		170	" ",// "MAXDrift ", //20
		171	"N_Speed ",
		172	"E_Speed ",
99	killagreg	173	"P-Part ",
		174	"I-Part ",
		175	"D-Part ",//25
		176	"PID-Part ",
41	ingob	177	"Distance N ",
		178	"Distance E ",
		179	"GPS_Nick ",
		180	"GPS_Roll ", //30
		181	"Used_Sats "
		182	};
		183
		184
		185	/********************************************************/
		186	/* Initialization the UART1 */
		187	/********************************************************/
		188	void UART1_Init (void)
		189	{
		190	GPIO_InitTypeDef GPIO_InitStructure;
		191	UART_InitTypeDef UART_InitStructure;
		192
		193	SCU_APBPeriphClockConfig(__UART1, ENABLE); // Enable the UART1 Clock
		194	SCU_APBPeriphClockConfig(__GPIO3, ENABLE); // Enable the GPIO3 Clock
		195
		196	/Configure UART1_Rx pin GPIO3.2/
		197	GPIO_StructInit(&GPIO_InitStructure);
		198	GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
		199	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
		200	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;
		201	GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
		202	GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; // UART1_RxD
		203	GPIO_Init(GPIO3, &GPIO_InitStructure);
		204
		205	/Configure UART1_Tx pin GPIO3.3/
		206	GPIO_StructInit(&GPIO_InitStructure);
		207	GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
		208	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
		209	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;
		210	GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; // UART1_TX
		211	GPIO_Init(GPIO3, &GPIO_InitStructure);
		212
		213	/* UART1 configured as follow:
		214	- Word Length = 8 Bits
		215	- One Stop Bit
		216	- No parity
		217	- BaudRate = 57600 baud
		218	- Hardware flow control Disabled
		219	- Receive and transmit enabled
		220	- Receive and transmit FIFOs are Disabled
		221	*/
		222	UART_StructInit(&UART_InitStructure);
		223	UART_InitStructure.UART_WordLength = UART_WordLength_8D;
		224	UART_InitStructure.UART_StopBits = UART_StopBits_1;
		225	UART_InitStructure.UART_Parity = UART_Parity_No ;
		226	UART_InitStructure.UART_BaudRate = BAUD_RATE;
		227	UART_InitStructure. UART_HardwareFlowControl = UART_HardwareFlowControl_None;
		228	UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx;
		229	UART_InitStructure.UART_FIFO = UART_FIFO_Enable;
		230	UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2;
		231	UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2;
		232
		233	UART_DeInit(UART1); // reset uart 1 to default
		234	UART_Init(UART1, &UART_InitStructure); // initialize uart 1
		235	// enable uart 1 interrupts selective
		236	UART_ITConfig(UART1, UART_IT_Receive \| UART_IT_ReceiveTimeOut, ENABLE);
		237	UART_Cmd(UART1, ENABLE); // enable uart 1
		238	// configure the uart 1 interupt line as an IRQ with priority 4 (0 is highest)
		239	VIC_Config(UART1_ITLine, VIC_IRQ, 4);
		240	// enable the uart 1 IRQ
		241	VIC_ITCmd(UART1_ITLine, ENABLE);
		242	// initialize the debug timer
		243	DebugData_Timer = SetDelay(DebugData_Interval);
54	killagreg	244	NaviData_Timer = SetDelay(NaviData_Interval)+500;
41	ingob	245	// unlock rxd_buffer
		246	rxd_buffer_locked = FALSE;
		247	pRxData = NULL;
		248	RxDataLen = 0;
		249	// no bytes to send
		250	txd_complete = TRUE;
		251	// Fill Version Info Structure
		252	UART_VersionInfo.SWMajor = VERSION_MAJOR;
		253	UART_VersionInfo.SWMinor = VERSION_MINOR;
		254	UART_VersionInfo.SWPatch = VERSION_PATCH;
		255	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
		256	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
		257
89	killagreg	258	NaviData.Version = NAVIDATA_VERSION;
92	killagreg	259
41	ingob	260	SerialPutString("\r\nUART1 init...ok");
		261	}
		262
		263
		264	/****************************************************************/
		265	/* USART1 receiver ISR */
		266	/****************************************************************/
		267	void UART1_IRQHandler(void)
		268	{
		269	static u16 crc;
		270	static u8 ptr_rxd_buffer = 0;
		271	static u8 crc1, crc2;
		272	static u8 abortState = 0;
		273	u8 c;
		274
		275	if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) \|\| (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )
		276	{
		277	// clear the pending bits
		278	UART_ClearITPendingBit(UART1, UART_IT_Receive);
		279	UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);
		280	// if debug UART is not UART1
		281	if (DebugUART != UART1)
		282	{ // forward received data to the debug UART tx buffer
		283	while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
		284	{
		285	// wait for space in the tx buffer of the DebugUART
		286	while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};
		287	// move the byte from the rx buffer of UART1 to the tx buffer of DebugUART
		288	c = UART_ReceiveData(UART1);
		289
		290	// check for abort condition (ESC ESC 0x55 0xAA 0x00)
		291	switch (abortState)
		292	{
		293	case 0: if (c == 27) abortState++;
		294	break;
		295	case 1: if (c == 27) abortState++; else abortState = 0;
		296	break;
		297	case 2: if (c == 0x55) abortState++; else abortState = 0;
		298	break;
		299	case 3: if (c == 0xAA) abortState++; else abortState = 0;
		300	break;
		301	case 4: if (c == 0x00)
		302	{
		303	DebugUART = UART1;
		304	UART0_Connect_to_MKGPS();
		305	}
		306	abortState = 0;
		307	break;
		308	}
		309
		310	if (DebugUART != UART1) UART_SendData(DebugUART, c);
		311	}
		312	}
		313	else // DebugUART == UART1 (normal operation)
		314	{
		315	while ((UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET) && (!rxd_buffer_locked))
		316	{ // some byes in the fifo and rxd buffer not locked
		317	// get byte from fifo
		318	c = UART_ReceiveData(UART1);
		319	if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
		320	{
		321	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
		322	crc = c; // init crc
		323	}
		324	#if 0
		325	else if (ptr_rxd_buffer == 1) // handle address
		326	{
		327	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
		328	crc += c; // update crc
		329	}
		330	#endif
		331	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // rxd buffer not full
		332	{
		333	if (c != '\r') // no termination character received
		334	{
		335	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
		336	crc += c; // update crc
		337	}
		338	else // termination character received
		339	{
		340	// the last 2 bytes are no subject for checksum calculation
		341	// they are the checksum itself
		342	crc -= rxd_buffer[ptr_rxd_buffer-2];
		343	crc -= rxd_buffer[ptr_rxd_buffer-1];
		344	// calculate checksum from transmitted data
		345	crc %= 4096;
		346	crc1 = '=' + crc / 64;
		347	crc2 = '=' + crc % 64;
		348	// compare checksum to transmitted checksum bytes
		349	if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
		350	{ // checksum valid
		351	rxd_buffer_locked = TRUE; // lock the rxd buffer
		352	ReceivedBytes = ptr_rxd_buffer + 1; // store number of received bytes
		353	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
		354	// if 2nd byte is an 'R' start bootloader
		355	if(rxd_buffer[2] == 'R')
		356	{
		357	PowerOff();
		358	VIC_DeInit();
		359	Execute_Bootloader(); // Reset-Commando - Bootloader starten
		360	}
		361	} // eof checksum valid
		362	else
		363	{ // checksum invalid
		364	rxd_buffer_locked = FALSE; // unlock rxd buffer
		365	} // eof checksum invalid
		366	ptr_rxd_buffer = 0; // reset rxd buffer pointer
		367	} // eof termination character received
		368	} // rxd buffer not full
		369	else // rxd buffer overrun
		370	{
		371	ptr_rxd_buffer = 0; // reset rxd buffer pointer
		372	rxd_buffer_locked = FALSE; // unlock rxd buffer
		373	} // eof rxd buffer overrrun
		374	} // some byes in the fifo and rxd buffer not locked
		375	} // eof DebugUart = UART1
		376	}
		377	}
		378
		379	/**************************************************************/
		380	/* Transmit tx buffer via debug uart */
		381	/**************************************************************/
		382	void UART1_Transmit(void)
		383	{
		384	u8 tmp_tx;
		385	// if something has to be send and the txd fifo is not full
		386	if((!txd_complete) && (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) == RESET))
		387	{
		388	tmp_tx = txd_buffer[ptr_txd_buffer]; // read byte from txd buffer
		389	// if terminating character or end of txd buffer reached
		390	if((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN))
		391	{
		392	ptr_txd_buffer = 0; // reset txd buffer pointer
		393	txd_complete = TRUE;// set complete flag
		394	}
		395	UART_SendData(UART1, tmp_tx); // put character to txd fifo
		396	// set pointer to next byte
		397	ptr_txd_buffer++;
		398	}
		399	}
		400
		401	/**************************************************************/
		402	/* Add CRC and initiate transmission via debug uart */
		403	/**************************************************************/
		404	void AddCRC(u16 datalen)
		405	{
		406	u16 tmpCRC = 0, i;
		407	for(i = 0; i < datalen; i++)
		408	{
		409	tmpCRC += txd_buffer[i];
		410	}
		411	tmpCRC %= 4096;
		412	txd_buffer[i++] = '=' + tmpCRC / 64;
		413	txd_buffer[i++] = '=' + tmpCRC % 64;
		414	txd_buffer[i++] = '\r';
		415
		416	ptr_txd_buffer = 0;
		417	txd_complete = FALSE;
49	ingob	418	UART_SendData(UART1, txd_buffer[ptr_txd_buffer++]); // send first byte, to be continued in the txd irq
41	ingob	419	}
		420
		421
		422
		423	/**************************************************************/
		424	/* Code output data */
		425	/**************************************************************/
		426	void SendOutData(u8 cmd, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....
		427	{
		428	va_list ap;
		429
		430	u16 pt = 0;
		431	u8 a,b,c;
		432	u8 ptr = 0;
		433
		434	u8* pdata = NULL;
		435	int len = 0;
		436
		437	txd_buffer[pt++] = '#'; // Start character
		438	txd_buffer[pt++] = 'a' + Address; // Address (a=0; b=1,...)
		439	txd_buffer[pt++] = cmd; // Command
		440
		441	va_start(ap, numofbuffers);
		442	if(numofbuffers)
		443	{
		444	pdata = va_arg(ap, u8*);
		445	len = va_arg(ap, int);
		446	ptr = 0;
		447	numofbuffers--;
		448	}
		449	while(len)
		450	{
		451	if(len)
		452	{
		453	a = pdata[ptr++];
		454	len--;
		455	if((!len) && numofbuffers) // try to jump to next buffer
		456	{
		457	pdata = va_arg(ap, u8*);
		458	len = va_arg(ap, int);
		459	ptr = 0;
		460	numofbuffers--;
		461	}
		462	}
		463	else a = 0;
		464	if(len)
		465	{
		466	b = pdata[ptr++];
		467	len--;
		468	if((!len) && numofbuffers) // try to jump to next buffer
		469	{
		470	pdata = va_arg(ap, u8*);
		471	len = va_arg(ap, int);
		472	ptr = 0;
		473	numofbuffers--;
		474	}
		475	}
		476	else b = 0;
		477	if(len)
		478	{
		479	c = pdata[ptr++];
		480	len--;
		481	if((!len) && numofbuffers) // try to jump to next buffer
		482	{
		483	pdata = va_arg(ap, u8*);
		484	len = va_arg(ap, int);
		485	ptr = 0;
		486	numofbuffers--;
		487	}
		488	}
		489	else c = 0;
		490	txd_buffer[pt++] = '=' + (a >> 2);
		491	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));
		492	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));
		493	txd_buffer[pt++] = '=' + ( c & 0x3f);
		494	}
		495	va_end(ap);
		496	AddCRC(pt); // add checksum after data block and initates the transmission
		497	}
		498
		499
		500	/**************************************************************/
		501	/* Decode data */
		502	/**************************************************************/
		503	void Decode64(void)
		504	{
		505	u8 a,b,c,d;
		506	u8 x,y,z;
		507	u8 ptrIn = 3; // start with first data byte in rx buffer
		508	u8 ptrOut = 3;
		509	u8 len = ReceivedBytes - 6; // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )
		510	while(len)
		511	{
		512	a = rxd_buffer[ptrIn++] - '=';
		513	b = rxd_buffer[ptrIn++] - '=';
		514	c = rxd_buffer[ptrIn++] - '=';
		515	d = rxd_buffer[ptrIn++] - '=';
		516	//if(ptrIn > ReceivedBytes - 3) break;
		517
		518	x = (a << 2) \| (b >> 4);
		519	y = ((b & 0x0f) << 4) \| (c >> 2);
		520	z = ((c & 0x03) << 6) \| d;
		521
		522	if(len--) rxd_buffer[ptrOut++] = x; else break;
		523	if(len--) rxd_buffer[ptrOut++] = y; else break;
		524	if(len--) rxd_buffer[ptrOut++] = z; else break;
		525	}
		526	pRxData = &rxd_buffer[3];
		527	RxDataLen = ptrOut - 3;
		528	}
		529
		530	/**************************************************************/
		531	/* Process incomming data from debug uart */
		532	/**************************************************************/
		533	void UART1_ProcessRxData(void)
		534	{
		535	// if data in the rxd buffer are not locked immediately return
		536	if((!rxd_buffer_locked) \|\| (DebugUART != UART1) ) return;
		537	Waypoint_t * pWaypoint = NULL;
		538
		539	PcAccess = 255;
		540	Decode64(); // decode data block in rxd buffer
101	holgerb	541	if(rxd_buffer[2] != 'z') SerialLinkOkay = 250; // reset SerialTimeout, but not in case of the "ping"
41	ingob	542	switch(rxd_buffer[1] - 'a') // check for Slave Address
		543	{
		544	case NC_ADDRESS: // own Slave Address
		545	switch(rxd_buffer[2])
		546	{
101	holgerb	547	case 'z': // connection checker
96	killagreg	548	memcpy(&Echo, (u8*)pRxData, sizeof(Echo)); // copy echo pattern
92	killagreg	549	Request_Echo = TRUE;
		550	break;
		551
41	ingob	552	case 'e': // request for the text of the error status
		553	Request_ErrorMessage = TRUE;
		554	break;
		555
		556	case 's':// new target position
		557	pWaypoint = (Waypoint_t*)&pRxData[0];
		558	BeepTime = 300;
		559	if(pWaypoint->Position.Status == NEWDATA)
		560	{
		561	WPList_Clear(); // empty WPList
		562	WPList_Append(pWaypoint);
		563	GPS_pWaypoint = WPList_Begin();
		564	}
		565	break;
		566
		567	case 'u': // redirect debug uart
		568	switch(pRxData[0])
		569	{
		570	case UART_FLIGHTCTRL:
		571	UART2_Init(); // initialize UART2 to FC pins
		572	DebugUART = UART2;
		573	break;
		574	case UART_MK3MAG:
		575	if(FC.MKFlags & MKFLAG_MOTOR_RUN) break; // not if the motors are running
		576	UART0_Connect_to_MK3MAG(); // mux UART0 to MK3MAG pins
		577	GPSData.Status = INVALID;
		578	DebugUART = UART0;
		579	break;
		580	case UART_MKGPS:
		581	if(FC.MKFlags & MKFLAG_MOTOR_RUN) break; // not if the motors are running
		582	UART0_Connect_to_MKGPS(); // connect UART0 to MKGPS pins
		583	GPSData.Status = INVALID;
		584	DebugUART = UART0;
		585	break;
		586	}
		587	break;
		588
92	killagreg	589	case 'w':// Append Waypoint to List
41	ingob	590	pWaypoint = (Waypoint_t*)&pRxData[0];
		591	if(pWaypoint->Position.Status == INVALID)
		592	{ // clear WP List
		593	WPList_Clear();
		594	GPS_pWaypoint = WPList_Begin();
		595	//SerialPutString("\r\nClear WP List\r\n");
		596	}
		597	else if (pWaypoint->Position.Status == NEWDATA)
		598	{ // app current WP to the list
		599	WPList_Append(pWaypoint);
		600	BeepTime = 500;
		601	//SerialPutString("\r\nAdd WP to List\r\n");
		602	}
		603	Request_NewWaypoint = TRUE;
		604	break;
		605
92	killagreg	606	case 'x':// Read Waypoint from List
		607	Request_ReadWaypoint = pRxData[0];
		608	break;
		609
41	ingob	610	default:
		611	// unsupported command recieved
		612	break;
		613	} // case NC_ADDRESS
65	killagreg	614	// "break;" is missing here to fall thru to the common commands
41	ingob	615
		616	default: // and any other Slave Address
		617
		618	switch(rxd_buffer[2]) // check CmdID
		619	{
		620	case 'a':// request for the labels of the analog debug outputs
		621	Request_DebugLabel = pRxData[0];
		622	if(Request_DebugLabel > 31) Request_DebugLabel = 31;
		623	break;
		624
		625	case 'b': // submit extern control
		626	memcpy(&ExternControl, (u8*)&pRxData[0], sizeof(ExternControl));
		627	ConfirmFrame = ExternControl.Frame;
		628	break;
		629
		630	case 'd': // request for debug data;
		631	DebugData_Interval = (u32) pRxData[0] * 10;
		632	if(DebugData_Interval > 0) Request_DebugData = TRUE;
		633	break;
		634
63	killagreg	635	case 'c': // request for 3D data;
		636	Data3D_Interval = (u32) pRxData[0] * 10;
		637	if(Data3D_Interval > 0) Request_Data3D = TRUE;
		638	break;
		639
41	ingob	640	case 'g':// request for external control data
		641	Request_ExternalControl = TRUE;
		642	break;
		643
		644	case 'h':// reqest for display line
		645	RemoteKeys \|= pRxData[0];
		646	if(RemoteKeys != 0) DisplayLine = 0;
		647	Request_Display = TRUE;
		648	break;
		649
		650	case 'l':// reqest for display columns
		651	MenuItem = pRxData[0];
		652	Request_Display1 = TRUE;
		653	break;
64	holgerb	654
		655	case 'o': // request for navigation information
		656	NaviData_Interval = (u32) pRxData[0] * 10;
		657	if(NaviData_Interval > 0) Request_NaviData = TRUE;
		658	break;
		659
41	ingob	660	case 'v': // request for version info
		661	Request_VerInfo = TRUE;
		662	break;
		663	default:
		664	// unsupported command recieved
		665	break;
		666	}
		667	break; // default:
		668	}
		669	// unlock the rxd buffer after processing
		670	pRxData = NULL;
		671	RxDataLen = 0;
		672	rxd_buffer_locked = FALSE;
		673	}
		674
		675
		676	/*****************************************************/
		677	/* Send a character */
		678	/*****************************************************/
		679	s16 uart_putchar (char c)
		680	{
		681	if (c == '\n') uart_putchar('\r');
		682	// wait until txd fifo is not full
		683	while (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != RESET);
		684	// transmit byte
		685	UART_SendData(UART1, c);
		686	return (0);
		687	}
		688
		689	/*****************************************************/
		690	/* Send a string to the debug uart */
		691	/*****************************************************/
		692	void SerialPutString(u8 *s)
		693	{
		694	if(s == NULL) return;
		695	while (*s != '\0' && DebugUART == UART1)
		696	{
		697	uart_putchar(*s);
		698	s ++;
		699	}
		700	}
		701
		702
		703
		704	/**************************************************************/
		705	/* Send the answers to incomming commands at the debug uart */
		706	/**************************************************************/
		707	void UART1_TransmitTxData(void)
		708	{
		709	if(!txd_complete \|\| (DebugUART != UART1) ) return;
		710
92	killagreg	711	if(Request_Echo && txd_complete)
		712	{
		713	SendOutData('Z', NC_ADDRESS, 1, &Echo, sizeof(Echo)); // answer the echo request
		714	Echo = 0; // reset echo value
		715	Request_Echo = FALSE;
		716	}
41	ingob	717	if(Request_DebugLabel != 0xFF)
		718	{
		719	SendOutData('A', NC_ADDRESS, 2, &Request_DebugLabel, sizeof(Request_DebugLabel), (u8 *) ANALOG_LABEL[Request_DebugLabel], 16);
		720	Request_DebugLabel = 0xFF;
		721	}
		722	if(ConfirmFrame && txd_complete)
		723	{
		724	SendOutData('B', NC_ADDRESS, 1, &ConfirmFrame, sizeof(ConfirmFrame));
		725	ConfirmFrame = 0;
		726	}
62	killagreg	727	if( (( (DebugData_Interval > 0) && CheckDelay(DebugData_Timer)) \|\| Request_DebugData) && txd_complete)
41	ingob	728	{
		729	SendOutData('D', NC_ADDRESS, 1,(u8 *)&DebugOut, sizeof(DebugOut));
		730	DebugData_Timer = SetDelay(DebugData_Interval);
		731	Request_DebugData = FALSE;
		732	}
61	holgerb	733
62	killagreg	734	if((( (Data3D_Interval > 0) && CheckDelay(Data3D_Timer) ) \|\| Request_Data3D) && txd_complete)
61	holgerb	735	{
		736	SendOutData('C', NC_ADDRESS, 1,(u8 *)&Data3D, sizeof(Data3D));
62	killagreg	737	Data3D_Timer = SetDelay(Data3D_Interval);
		738	Request_Data3D = FALSE;
61	holgerb	739	}
		740
41	ingob	741	if(Request_ExternalControl && txd_complete)
		742	{
		743	SendOutData('G', NC_ADDRESS, 1, (u8 *)&ExternControl, sizeof(ExternControl));
		744	Request_ExternalControl = FALSE;
		745	}
		746	if(Request_Display && txd_complete)
		747	{
		748	LCD_PrintMenu();
		749	SendOutData('H', NC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), (u8)&DisplayBuff[DisplayLine 20], 20);
		750	DisplayLine++;
		751	if(DisplayLine >= 4) DisplayLine = 0;
		752	Request_Display = FALSE;
		753	}
		754	if(Request_Display1 && txd_complete)
		755	{
		756	LCD_PrintMenu();
		757	SendOutData('L', NC_ADDRESS, 3, (u8)&MenuItem, sizeof(MenuItem), (u8)&MaxMenuItem, sizeof(MaxMenuItem),(u8*)DisplayBuff, sizeof(DisplayBuff));
		758	Request_Display1 = FALSE;
		759	}
		760	if(Request_VerInfo && txd_complete)
		761	{
		762	SendOutData('V', NC_ADDRESS,1, (u8 *)&UART_VersionInfo, sizeof(UART_VersionInfo));
		763	Request_VerInfo = FALSE;
		764	}
		765	if(( (NaviData_Interval && CheckDelay(NaviData_Timer) ) \|\| Request_NaviData) && txd_complete)
		766	{
		767	NaviData.Errorcode = ErrorCode;
65	killagreg	768	SendOutData('O', NC_ADDRESS,1, (u8 *)&NaviData, sizeof(NaviData));
		769	if (DebugUART == UART1) SendOutData0('O', NC_ADDRESS,1, (u8 *)&NaviData, sizeof(NaviData));
41	ingob	770	NaviData_Timer = SetDelay(NaviData_Interval);
		771	Request_NaviData = FALSE;
		772	}
		773	if(Request_ErrorMessage && txd_complete)
		774	{
		775	SendOutData('E', NC_ADDRESS, 1, (u8 *)&ErrorMSG, sizeof(ErrorMSG));
		776	Request_ErrorMessage = FALSE;
		777	}
		778	if(Request_SendFollowMe && txd_complete && (GPSData.NumOfSats >= 4)) // sending for "Follow me"
		779	{
		780	GPS_CopyPosition(&(GPSData.Position),&(FollowMe.Position));
		781	FollowMe.Position.Status = NEWDATA;
		782	FollowMe.Heading = -1;
		783	FollowMe.ToleranceRadius = 1;
		784	FollowMe.HoldTime = 60;
		785	FollowMe.Event_Flag = 0;
		786	FollowMe.reserve[0] = 0; // reserve
		787	FollowMe.reserve[1] = 0; // reserve
		788	FollowMe.reserve[2] = 0; // reserve
		789	FollowMe.reserve[3] = 0; // reserve
		790	SendOutData('s', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe));
		791	Request_SendFollowMe = FALSE;
		792	}
		793
		794	if(Request_NewWaypoint && txd_complete)
		795	{
		796	u8 WPNumber = WPList_GetCount();
65	killagreg	797	SendOutData('W', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
41	ingob	798	Request_NewWaypoint = FALSE;
		799	}
		800
92	killagreg	801	if((Request_ReadWaypoint != 0xFF) && txd_complete)
		802	{
		803	u8 WPNumber = WPList_GetCount();
		804	if (Request_ReadWaypoint < WPNumber)
		805	{
		806	SendOutData('X', NC_ADDRESS, 3, &WPNumber, 1, &Request_ReadWaypoint, 1, WPList_GetAt(Request_ReadWaypoint), sizeof(Waypoint_t));
		807	}
		808	else
		809	{
		810	SendOutData('X', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
		811	}
		812	Request_ReadWaypoint = 0xFF;
		813	}
		814
41	ingob	815	}
		816

Subversion Repositories NaviCtrl

(root)/tags/V0.15c/uart1.c - Rev 171