Subversion Repositories svnkaklik

/*! \file tsip.c \brief TSIP (Trimble Standard Interface Protocol) function library. */

//*****************************************************************************

//

// File Name	: 'tsip.c'

// Title		: TSIP (Trimble Standard Interface Protocol) function library

// Author		: Pascal Stang - Copyright (C) 2002-2003

// Created		: 2002.08.27

// Revised		: 2003.07.17

// Version		: 0.1

// Target MCU	: Atmel AVR Series

// Editor Tabs	: 4

//

// NOTE: This code is currently below version 1.0, and therefore is considered

// to be lacking in some functionality or documentation, or may not be fully

// tested.  Nonetheless, you can expect most functions to work.

//

// This code is distributed under the GNU Public License

//		which can be found at http://www.gnu.org/licenses/gpl.txt

//

//*****************************************************************************

#ifndef WIN32

	#include <avr/io.h>

	#include <avr/pgmspace.h>

	#include <math.h>

	#include <stdlib.h>

#endif

#include "global.h"

#include "buffer.h"

#include "rprintf.h"

#include "uart2.h"

#include "gps.h"

#include "tsip.h"

// Program ROM constants

// Global variables

extern GpsInfoType GpsInfo;

#define BUFFERSIZE 0x40

u08 TsipPacket[BUFFERSIZE];

u08 debug;

// function pointer to single byte output routine

static void (*TsipTxByteFunc)(unsigned char c);

void tsipInit(void (*txbytefunc)(unsigned char c))

{

	// set transmit function

	// (this function will be used for all SendPacket commands)

	TsipTxByteFunc = txbytefunc;

	// set debug status

	debug = 0;

	// compose GPS receiver configuration packet

	u08 packet[4];

	packet[0] = BV(POS_LLA);

	packet[1] = BV(VEL_ENU);

	packet[2] = 0;

	packet[3] = 0;

	// send configuration

	tsipSendPacket(TSIPTYPE_SET_IO_OPTIONS, 4, packet);

}

void tsipSendPacket(u08 tsipType, u08 dataLength, u08* data)

{

	u08 i;

	u08 dataIdx = 0;

	// start of packet

	TsipPacket[dataIdx++] = DLE;

	// packet type

	TsipPacket[dataIdx++] = tsipType;

	// add packet data

	for(i=0; i<dataLength; i++)

	{

		if(*data == DLE)

		{

			// do double-DLE escape sequence

			TsipPacket[dataIdx++] = *data;

			TsipPacket[dataIdx++] = *data++;

		}

		else

			TsipPacket[dataIdx++] = *data++;

	}

	// end of packet

	TsipPacket[dataIdx++] = DLE;

	TsipPacket[dataIdx++] = ETX;

	for(i=0; i<dataIdx; i++)

		TsipTxByteFunc(TsipPacket[i]);

}

u08 tsipProcess(cBuffer* rxBuffer)

{

	u08 foundpacket = FALSE;

	u08 startFlag = FALSE;

	u08 data;

	u08 i,j,k;

	u08 TsipPacketIdx;

	// process the receive buffer

	// go through buffer looking for packets

	while(rxBuffer->datalength > 1)

	{

		// look for a potential start of TSIP packet

		if(bufferGetAtIndex(rxBuffer,0) == DLE)

		{

			// make sure the next byte is not DLE or ETX

			data = bufferGetAtIndex(rxBuffer,1);

			if((data != DLE) && (data != ETX))

			{

				// found potential start

				startFlag = TRUE;

				// done looking for start

				break;

			}

		}

		else

			// not DLE, dump character from buffer

			bufferGetFromFront(rxBuffer);

	}

	// if we detected a start, look for end of packet

	if(startFlag)

	{

		for(i=1; i<(rxBuffer->datalength)-1; i++)

		{

			// check for potential end of TSIP packet

			if((bufferGetAtIndex(rxBuffer,i) == DLE) && (bufferGetAtIndex(rxBuffer,i+1) == ETX))

			{

				// have a packet end

				// dump initial DLE

				bufferGetFromFront(rxBuffer);

				// copy data to TsipPacket

				TsipPacketIdx = 0;

				for(j=0; j<(i-1); j++)

				{

					data = bufferGetFromFront(rxBuffer);

					if(data == DLE)

					{

						if(bufferGetAtIndex(rxBuffer,0) == DLE)

						{

							// found double-DLE escape sequence, skip one of them

							bufferGetFromFront(rxBuffer);

							j++;

						}

					}

					TsipPacket[TsipPacketIdx++] = data;

				}

				// dump ending DLE+ETX

				bufferGetFromFront(rxBuffer);

				bufferGetFromFront(rxBuffer);

				// found a packet

				if(debug)

				{

					rprintf("Rx TSIP packet type: 0x%x  len: %d  rawlen: %d\r\n",

						TsipPacket[0],

						TsipPacketIdx,

						i);

					for(k=0; k<TsipPacketIdx; k++)

					{

						rprintfu08(TsipPacket[k]);

						rprintfChar(' ');

					}

					//rprintfu08(bufferGetFromFront(rxBuffer)); rprintfChar(' ');

					//rprintfu08(bufferGetFromFront(rxBuffer)); rprintfChar(' ');

					rprintfCRLF();

				}

				// done with this processing session

				foundpacket = TRUE;

				break;

			}

		}

	}

	if(foundpacket)

	{

		// switch on the packet type

		switch(TsipPacket[0])

		{

		case TSIPTYPE_GPSTIME:			tsipProcessGPSTIME(TsipPacket); break;

		case TSIPTYPE_POSFIX_XYZ_SP:	tsipProcessPOSFIX_XYZ_SP(TsipPacket); break;

		case TSIPTYPE_VELFIX_XYZ:		tsipProcessVELFIX_XYZ(TsipPacket); break;

		case TSIPTYPE_POSFIX_LLA_SP:	tsipProcessPOSFIX_LLA_SP(TsipPacket); break;

		case TSIPTYPE_VELFIX_ENU:		tsipProcessVELFIX_ENU(TsipPacket); break;

		case TSIPTYPE_RAWDATA: break;

		default:

			//if(debug) rprintf("Unhandled TSIP packet type: 0x%x\r\n",TsipPacket[0]);

			break;

		}

	}

	return foundpacket;

}

void tsipProcessGPSTIME(u08* packet)

{

	// NOTE: check endian-ness if porting to processors other than the AVR

	GpsInfo.TimeOfWeek.b[3] = packet[1];

	GpsInfo.TimeOfWeek.b[2] = packet[2];

	GpsInfo.TimeOfWeek.b[1] = packet[3];

	GpsInfo.TimeOfWeek.b[0] = packet[4];

	GpsInfo.WeekNum	= ((u16)packet[5]<<8)|((u16)packet[6]);

	GpsInfo.UtcOffset.b[3]	= packet[7];

	GpsInfo.UtcOffset.b[2]	= packet[8];

	GpsInfo.UtcOffset.b[1]	= packet[9];

	GpsInfo.UtcOffset.b[0]	= packet[10];

}

void tsipProcessPOSFIX_XYZ_SP(u08* packet)

{

	// NOTE: check endian-ness if porting to processors other than the AVR

	GpsInfo.PosECEF.x.b[3] = packet[1];

	GpsInfo.PosECEF.x.b[2] = packet[2];

	GpsInfo.PosECEF.x.b[1] = packet[3];

	GpsInfo.PosECEF.x.b[0] = packet[4];

	GpsInfo.PosECEF.y.b[3] = packet[5];

	GpsInfo.PosECEF.y.b[2] = packet[6];

	GpsInfo.PosECEF.y.b[1] = packet[7];

	GpsInfo.PosECEF.y.b[0] = packet[8];

	GpsInfo.PosECEF.z.b[3] = packet[9];

	GpsInfo.PosECEF.z.b[2] = packet[10];

	GpsInfo.PosECEF.z.b[1] = packet[11];

	GpsInfo.PosECEF.z.b[0] = packet[12];

	GpsInfo.PosECEF.TimeOfFix.b[3] = packet[13];

	GpsInfo.PosECEF.TimeOfFix.b[2] = packet[14];

	GpsInfo.PosECEF.TimeOfFix.b[1] = packet[15];

	GpsInfo.PosECEF.TimeOfFix.b[0] = packet[16];

	GpsInfo.PosECEF.updates++;

//	GpsInfo.TimeOfFix_ECEF.f = *((float*)&packet[13]);

}

void tsipProcessVELFIX_XYZ(u08* packet)

{

}

void tsipProcessPOSFIX_LLA_SP(u08* packet)

{

	// NOTE: check endian-ness if porting to processors other than the AVR

	GpsInfo.PosLLA.lat.b[3] = packet[1];

	GpsInfo.PosLLA.lat.b[2] = packet[2];

	GpsInfo.PosLLA.lat.b[1] = packet[3];

	GpsInfo.PosLLA.lat.b[0] = packet[4];

	GpsInfo.PosLLA.lon.b[3] = packet[5];

	GpsInfo.PosLLA.lon.b[2] = packet[6];

	GpsInfo.PosLLA.lon.b[1] = packet[7];

	GpsInfo.PosLLA.lon.b[0] = packet[8];

	GpsInfo.PosLLA.alt.b[3] = packet[9];

	GpsInfo.PosLLA.alt.b[2] = packet[10];

	GpsInfo.PosLLA.alt.b[1] = packet[11];

	GpsInfo.PosLLA.alt.b[0] = packet[12];

	GpsInfo.PosLLA.TimeOfFix.b[3] = packet[17];

	GpsInfo.PosLLA.TimeOfFix.b[2] = packet[18];

	GpsInfo.PosLLA.TimeOfFix.b[1] = packet[18];

	GpsInfo.PosLLA.TimeOfFix.b[0] = packet[20];

	GpsInfo.PosLLA.updates++;

}

void tsipProcessVELFIX_ENU(u08* packet)

{

	// NOTE: check endian-ness if porting to processors other than the AVR

	GpsInfo.VelENU.east.b[3] = packet[1];

	GpsInfo.VelENU.east.b[2] = packet[2];

	GpsInfo.VelENU.east.b[1] = packet[3];

	GpsInfo.VelENU.east.b[0] = packet[4];

	GpsInfo.VelENU.north.b[3] = packet[5];

	GpsInfo.VelENU.north.b[2] = packet[6];

	GpsInfo.VelENU.north.b[1] = packet[7];

	GpsInfo.VelENU.north.b[0] = packet[8];

	GpsInfo.VelENU.up.b[3] =	packet[9];

	GpsInfo.VelENU.up.b[2] =	packet[10];

	GpsInfo.VelENU.up.b[1] =	packet[11];

	GpsInfo.VelENU.up.b[0] =	packet[12];

	GpsInfo.VelENU.TimeOfFix.b[3] =	packet[17];

	GpsInfo.VelENU.TimeOfFix.b[2] =	packet[18];

	GpsInfo.VelENU.TimeOfFix.b[1] =	packet[19];

	GpsInfo.VelENU.TimeOfFix.b[0] =	packet[20];

	GpsInfo.VelENU.updates++;

}

void tsipProcessRAWDATA(cBuffer* packet)

{

/*

	char oft = 1;

	// process the data in TSIPdata

	unsigned char SVnum = TSIPdata[oft];

	unsigned __int32 SNR32 =		(TSIPdata[oft+5] << 24) + (TSIPdata[oft+6] << 16) + (TSIPdata[oft+7] << 8) + (TSIPdata[oft+8]);

	unsigned __int32 codephase32 =	(TSIPdata[oft+9] << 24) + (TSIPdata[oft+10] << 16) + (TSIPdata[oft+11] << 8) + (TSIPdata[oft+12]);

	unsigned __int32 doppler32 =	(TSIPdata[oft+13] << 24) + (TSIPdata[oft+14] << 16) + (TSIPdata[oft+15] << 8) + (TSIPdata[oft+16]);

	unsigned __int64 meastimeH32 =	(TSIPdata[oft+17] << 24) | (TSIPdata[oft+18] << 16) | (TSIPdata[oft+19] << 8) | (TSIPdata[oft+20]);

	unsigned __int64 meastimeL32 =	(TSIPdata[oft+21] << 24) | (TSIPdata[oft+22] << 16) | (TSIPdata[oft+23] << 8) | (TSIPdata[oft+24]);

	unsigned __int64 meastime64 =	(meastimeH32 << 32) | (meastimeL32);

	float SNR =			*((float*) &SNR32);

	float codephase =	*((float*) &codephase32);

	float doppler =		*((float*) &doppler32);

	double meastime =	*((double*) &meastime64);

	// output to screen

	printf("SV%2d SNR: %5.2f PH: %11.4f DOP: %11.4f TIME: %5.0I64f EPOCH: %7.2I64f\n",SVnum,SNR,codephase,doppler,meastime,meastime/1.5);

	//printf("SV%2d  SNR: %5.2f  PH: %10.4f  DOP: %10.4f  TIME: %I64x\n",SVnum,SNR,codephase,doppler,meastime64);

	// output to file

	fprintf( logfile, "%2d  %5.2f  %11.4f  %11.4f  %5.0I64f  %7.2I64f\n",SVnum,SNR,codephase,doppler,meastime,meastime/1.5);

*/

}