tsip.c

Go to the documentation of this file.

/*! \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);
*/
}

---