mirror of
https://github.com/esp8266/Arduino.git
synced 2025-06-20 21:01:25 +03:00
Initial Arduino IDE based on Processing.
This commit is contained in:
David A. Mellis
331
build/shared/lib/avrlib/tsip.c
Executable file
331
build/shared/lib/avrlib/tsip.c
Executable file
@ -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);
|
||||
*/
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user