Main Page | Modules | Data Structures | Directories | File List | Data Fields | Globals | Related Pages

# tsip.c

Go to the documentation of this file.

    00001 /*! \file tsip.c \brief TSIP (Trimble Standard Interface Protocol) function library. */
    00002 //*****************************************************************************
    00003 //
    00004 // File Name    : 'tsip.c'
    00005 // Title        : TSIP (Trimble Standard Interface Protocol) function library
    00006 // Author       : Pascal Stang - Copyright (C) 2002-2003
    00007 // Created      : 2002.08.27
    00008 // Revised      : 2003.07.17
    00009 // Version      : 0.1
    00010 // Target MCU   : Atmel AVR Series
    00011 // Editor Tabs  : 4
    00012 //
    00013 // NOTE: This code is currently below version 1.0, and therefore is considered
    00014 // to be lacking in some functionality or documentation, or may not be fully
    00015 // tested.  Nonetheless, you can expect most functions to work.
    00016 //
    00017 // This code is distributed under the GNU Public License
    00018 //      which can be found at http://www.gnu.org/licenses/gpl.txt
    00019 //
    00020 //*****************************************************************************
    00021
    00022 #ifndef WIN32
    00023     #include <avr/io.h>
    00024     #include <avr/pgmspace.h>
    00025     #include <math.h>
    00026     #include <stdlib.h>
    00027 #endif
    00028
    00029 #include "global.h"
    00030 #include "buffer.h"
    00031 #include "rprintf.h"
    00032 #include "uart2.h"
    00033 #include "gps.h"
    00034
    00035 #include "tsip.h"
    00036
    00037 // Program ROM constants
    00038
    00039 // Global variables
    00040 extern GpsInfoType GpsInfo;
    00041 #define BUFFERSIZE 0x40
    00042 u08 TsipPacket[BUFFERSIZE];
    00043 u08 debug;
    00044
    00045 // function pointer to single byte output routine
    00046 static void (*TsipTxByteFunc)(unsigned char c);
    00047
    00048 void tsipInit(void (*txbytefunc)(unsigned char c))
    00049 {
    00050     // set transmit function
    00051     // (this function will be used for all SendPacket commands)
    00052     TsipTxByteFunc = txbytefunc;
    00053
    00054     // set debug status
    00055     debug = 0;
    00056
    00057     // compose GPS receiver configuration packet
    00058     u08 packet[4];
    00059     packet[0] = BV(POS_LLA);
    00060     packet[1] = BV(VEL_ENU);
    00061     packet[2] = 0;
    00062     packet[3] = 0;
    00063     // send configuration
    00064     tsipSendPacket(TSIPTYPE_SET_IO_OPTIONS, 4, packet);
    00065 }
    00066
    00067 void tsipSendPacket(u08 tsipType, u08 dataLength, u08* data)
    00068 {
    00069     u08 i;
    00070     u08 dataIdx = 0;
    00071
    00072     // start of packet
    00073     TsipPacket[dataIdx++] = DLE;
    00074     // packet type
    00075     TsipPacket[dataIdx++] = tsipType;
    00076     // add packet data
    00077     for(i=0; i<dataLength; i++)
    00078     {
    00079         if(*data == DLE)
    00080         {
    00081             // do double-DLE escape sequence
    00082             TsipPacket[dataIdx++] = *data;
    00083             TsipPacket[dataIdx++] = *data++;
    00084         }
    00085         else
    00086             TsipPacket[dataIdx++] = *data++;
    00087     }
    00088     // end of packet
    00089     TsipPacket[dataIdx++] = DLE;
    00090     TsipPacket[dataIdx++] = ETX;
    00091
    00092     for(i=0; i<dataIdx; i++)
    00093         TsipTxByteFunc(TsipPacket[i]);
    00094 }
    00095
    00096 u08 tsipProcess(cBuffer* rxBuffer)
    00097 {
    00098     u08 foundpacket = FALSE;
    00099     u08 startFlag = FALSE;
    00100     u08 data;
    00101     u08 i,j,k;
    00102
    00103     u08 TsipPacketIdx;
    00104
    00105     // process the receive buffer
    00106     // go through buffer looking for packets
    00107     while(rxBuffer->datalength > 1)
    00108     {
    00109         // look for a potential start of TSIP packet
    00110         if(bufferGetAtIndex(rxBuffer,0) == DLE)
    00111         {
    00112             // make sure the next byte is not DLE or ETX
    00113             data = bufferGetAtIndex(rxBuffer,1);
    00114             if((data != DLE) && (data != ETX))
    00115             {
    00116                 // found potential start
    00117                 startFlag = TRUE;
    00118                 // done looking for start
    00119                 break;
    00120             }
    00121         }
    00122         else
    00123             // not DLE, dump character from buffer
    00124             bufferGetFromFront(rxBuffer);
    00125     }
    00126
    00127     // if we detected a start, look for end of packet
    00128     if(startFlag)
    00129     {
    00130         for(i=1; i<(rxBuffer->datalength)-1; i++)
    00131         {
    00132             // check for potential end of TSIP packet
    00133             if((bufferGetAtIndex(rxBuffer,i) == DLE) && (bufferGetAtIndex(rxBuffer,i+1) == ETX))
    00134             {
    00135                 // have a packet end
    00136                 // dump initial DLE
    00137                 bufferGetFromFront(rxBuffer);
    00138                 // copy data to TsipPacket
    00139                 TsipPacketIdx = 0;
    00140                 for(j=0; j<(i-1); j++)
    00141                 {
    00142                     data = bufferGetFromFront(rxBuffer);
    00143                     if(data == DLE)
    00144                     {
    00145                         if(bufferGetAtIndex(rxBuffer,0) == DLE)
    00146                         {
    00147                             // found double-DLE escape sequence, skip one of them
    00148                             bufferGetFromFront(rxBuffer);
    00149                             j++;
    00150                         }
    00151                     }
    00152                     TsipPacket[TsipPacketIdx++] = data;
    00153                 }
    00154                 // dump ending DLE+ETX
    00155                 bufferGetFromFront(rxBuffer);
    00156                 bufferGetFromFront(rxBuffer);
    00157
    00158                 // found a packet
    00159                 if(debug)
    00160                 {
    00161                     rprintf("Rx TSIP packet type: 0x%x  len: %d  rawlen: %d\r\n",
    00162                         TsipPacket[0],
    00163                         TsipPacketIdx,
    00164                         i);
    00165                     for(k=0; k<TsipPacketIdx; k++)
    00166                     {
    00167                         rprintfu08(TsipPacket[k]);
    00168                         rprintfChar(' ');
    00169                     }
    00170                     //rprintfu08(bufferGetFromFront(rxBuffer)); rprintfChar(' ');
    00171                     //rprintfu08(bufferGetFromFront(rxBuffer)); rprintfChar(' ');
    00172
    00173                     rprintfCRLF();
    00174                 }
    00175                 // done with this processing session
    00176                 foundpacket = TRUE;
    00177                 break;
    00178             }
    00179         }
    00180     }
    00181
    00182     if(foundpacket)
    00183     {
    00184         // switch on the packet type
    00185         switch(TsipPacket[0])
    00186         {
    00187         case TSIPTYPE_GPSTIME:          tsipProcessGPSTIME(TsipPacket); break;
    00188         case TSIPTYPE_POSFIX_XYZ_SP:    tsipProcessPOSFIX_XYZ_SP(TsipPacket); break;
    00189         case TSIPTYPE_VELFIX_XYZ:       tsipProcessVELFIX_XYZ(TsipPacket); break;
    00190
    00191         case TSIPTYPE_POSFIX_LLA_SP:    tsipProcessPOSFIX_LLA_SP(TsipPacket); break;
    00192         case TSIPTYPE_VELFIX_ENU:       tsipProcessVELFIX_ENU(TsipPacket); break;
    00193
    00194         case TSIPTYPE_RAWDATA: break;
    00195         default:
    00196             //if(debug) rprintf("Unhandled TSIP packet type: 0x%x\r\n",TsipPacket[0]);
    00197             break;
    00198         }
    00199     }
    00200
    00201     return foundpacket;
    00202 }
    00203
    00204 void tsipProcessGPSTIME(u08* packet)
    00205 {
    00206     // NOTE: check endian-ness if porting to processors other than the AVR
    00207     GpsInfo.TimeOfWeek.b[3] = packet[1];
    00208     GpsInfo.TimeOfWeek.b[2] = packet[2];
    00209     GpsInfo.TimeOfWeek.b[1] = packet[3];
    00210     GpsInfo.TimeOfWeek.b[0] = packet[4];
    00211
    00212     GpsInfo.WeekNum = ((u16)packet[5]<<8)|((u16)packet[6]);
    00213
    00214     GpsInfo.UtcOffset.b[3]  = packet[7];
    00215     GpsInfo.UtcOffset.b[2]  = packet[8];
    00216     GpsInfo.UtcOffset.b[1]  = packet[9];
    00217     GpsInfo.UtcOffset.b[0]  = packet[10];
    00218 }
    00219
    00220 void tsipProcessPOSFIX_XYZ_SP(u08* packet)
    00221 {
    00222     // NOTE: check endian-ness if porting to processors other than the AVR
    00223     GpsInfo.PosECEF.x.b[3] = packet[1];
    00224     GpsInfo.PosECEF.x.b[2] = packet[2];
    00225     GpsInfo.PosECEF.x.b[1] = packet[3];
    00226     GpsInfo.PosECEF.x.b[0] = packet[4];
    00227
    00228     GpsInfo.PosECEF.y.b[3] = packet[5];
    00229     GpsInfo.PosECEF.y.b[2] = packet[6];
    00230     GpsInfo.PosECEF.y.b[1] = packet[7];
    00231     GpsInfo.PosECEF.y.b[0] = packet[8];
    00232
    00233     GpsInfo.PosECEF.z.b[3] = packet[9];
    00234     GpsInfo.PosECEF.z.b[2] = packet[10];
    00235     GpsInfo.PosECEF.z.b[1] = packet[11];
    00236     GpsInfo.PosECEF.z.b[0] = packet[12];
    00237
    00238     GpsInfo.PosECEF.TimeOfFix.b[3] = packet[13];
    00239     GpsInfo.PosECEF.TimeOfFix.b[2] = packet[14];
    00240     GpsInfo.PosECEF.TimeOfFix.b[1] = packet[15];
    00241     GpsInfo.PosECEF.TimeOfFix.b[0] = packet[16];
    00242
    00243     GpsInfo.PosECEF.updates++;
    00244
    00245 //  GpsInfo.TimeOfFix_ECEF.f = *((float*)&packet[13]);
    00246 }
    00247
    00248 void tsipProcessVELFIX_XYZ(u08* packet)
    00249 {
    00250 }
    00251
    00252 void tsipProcessPOSFIX_LLA_SP(u08* packet)
    00253 {
    00254     // NOTE: check endian-ness if porting to processors other than the AVR
    00255     GpsInfo.PosLLA.lat.b[3] = packet[1];
    00256     GpsInfo.PosLLA.lat.b[2] = packet[2];
    00257     GpsInfo.PosLLA.lat.b[1] = packet[3];
    00258     GpsInfo.PosLLA.lat.b[0] = packet[4];
    00259
    00260     GpsInfo.PosLLA.lon.b[3] = packet[5];
    00261     GpsInfo.PosLLA.lon.b[2] = packet[6];
    00262     GpsInfo.PosLLA.lon.b[1] = packet[7];
    00263     GpsInfo.PosLLA.lon.b[0] = packet[8];
    00264
    00265     GpsInfo.PosLLA.alt.b[3] = packet[9];
    00266     GpsInfo.PosLLA.alt.b[2] = packet[10];
    00267     GpsInfo.PosLLA.alt.b[1] = packet[11];
    00268     GpsInfo.PosLLA.alt.b[0] = packet[12];
    00269
    00270     GpsInfo.PosLLA.TimeOfFix.b[3] = packet[17];
    00271     GpsInfo.PosLLA.TimeOfFix.b[2] = packet[18];
    00272     GpsInfo.PosLLA.TimeOfFix.b[1] = packet[18];
    00273     GpsInfo.PosLLA.TimeOfFix.b[0] = packet[20];
    00274
    00275     GpsInfo.PosLLA.updates++;
    00276 }
    00277
    00278 void tsipProcessVELFIX_ENU(u08* packet)
    00279 {
    00280     // NOTE: check endian-ness if porting to processors other than the AVR
    00281     GpsInfo.VelENU.east.b[3] = packet[1];
    00282     GpsInfo.VelENU.east.b[2] = packet[2];
    00283     GpsInfo.VelENU.east.b[1] = packet[3];
    00284     GpsInfo.VelENU.east.b[0] = packet[4];
    00285
    00286     GpsInfo.VelENU.north.b[3] = packet[5];
    00287     GpsInfo.VelENU.north.b[2] = packet[6];
    00288     GpsInfo.VelENU.north.b[1] = packet[7];
    00289     GpsInfo.VelENU.north.b[0] = packet[8];
    00290
    00291     GpsInfo.VelENU.up.b[3] =    packet[9];
    00292     GpsInfo.VelENU.up.b[2] =    packet[10];
    00293     GpsInfo.VelENU.up.b[1] =    packet[11];
    00294     GpsInfo.VelENU.up.b[0] =    packet[12];
    00295
    00296     GpsInfo.VelENU.TimeOfFix.b[3] = packet[17];
    00297     GpsInfo.VelENU.TimeOfFix.b[2] = packet[18];
    00298     GpsInfo.VelENU.TimeOfFix.b[1] = packet[19];
    00299     GpsInfo.VelENU.TimeOfFix.b[0] = packet[20];
    00300
    00301     GpsInfo.VelENU.updates++;
    00302 }
    00303
    00304 void tsipProcessRAWDATA(cBuffer* packet)
    00305 {
    00306 /*
    00307     char oft = 1;
    00308     // process the data in TSIPdata
    00309     unsigned char SVnum = TSIPdata[oft];
    00310     unsigned __int32 SNR32 =        (TSIPdata[oft+5] << 24) + (TSIPdata[oft+6] << 16) + (TSIPdata[oft+7] << 8) + (TSIPdata[oft+8]);
    00311     unsigned __int32 codephase32 =  (TSIPdata[oft+9] << 24) + (TSIPdata[oft+10] << 16) + (TSIPdata[oft+11] << 8) + (TSIPdata[oft+12]);
    00312     unsigned __int32 doppler32 =    (TSIPdata[oft+13] << 24) + (TSIPdata[oft+14] << 16) + (TSIPdata[oft+15] << 8) + (TSIPdata[oft+16]);
    00313
    00314     unsigned __int64 meastimeH32 =  (TSIPdata[oft+17] << 24) | (TSIPdata[oft+18] << 16) | (TSIPdata[oft+19] << 8) | (TSIPdata[oft+20]);
    00315     unsigned __int64 meastimeL32 =  (TSIPdata[oft+21] << 24) | (TSIPdata[oft+22] << 16) | (TSIPdata[oft+23] << 8) | (TSIPdata[oft+24]);
    00316     unsigned __int64 meastime64 =   (meastimeH32 << 32) | (meastimeL32);
    00317
    00318     float SNR =         *((float*) &SNR32);
    00319     float codephase =   *((float*) &codephase32);
    00320     float doppler =     *((float*) &doppler32);
    00321     double meastime =   *((double*) &meastime64);
    00322
    00323     // output to screen
    00324     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);
    00325     //printf("SV%2d  SNR: %5.2f  PH: %10.4f  DOP: %10.4f  TIME: %I64x\n",SVnum,SNR,codephase,doppler,meastime64);
    00326
    00327     // output to file
    00328     fprintf( logfile, "%2d  %5.2f  %11.4f  %11.4f  %5.0I64f  %7.2I64f\n",SVnum,SNR,codephase,doppler,meastime,meastime/1.5);
    00329 */
    00330 }
    00331