0,0 → 1,331 |
/*! \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); |
*/ |
} |
|