?lang_form? ?lang_select? ?lang_submit? ?lang_endform?
{HEADER END}
{BLAME START}

library

?curdirlinks? -

Blame information for rev 6

Line No. Rev Author Line
1 6 kaklik /*! \file tsip.c \brief TSIP (Trimble Standard Interface Protocol) function library. */
2 //*****************************************************************************
3 //
4 // File Name : 'tsip.c'
5 // Title : TSIP (Trimble Standard Interface Protocol) function library
6 // Author : Pascal Stang - Copyright (C) 2002-2003
7 // Created : 2002.08.27
8 // Revised : 2003.07.17
9 // Version : 0.1
10 // Target MCU : Atmel AVR Series
11 // Editor Tabs : 4
12 //
13 // NOTE: This code is currently below version 1.0, and therefore is considered
14 // to be lacking in some functionality or documentation, or may not be fully
15 // tested. Nonetheless, you can expect most functions to work.
16 //
17 // This code is distributed under the GNU Public License
18 // which can be found at http://www.gnu.org/licenses/gpl.txt
19 //
20 //*****************************************************************************
21  
22 #ifndef WIN32
23 #include <avr/io.h>
24 #include <avr/pgmspace.h>
25 #include <math.h>
26 #include <stdlib.h>
27 #endif
28  
29 #include "global.h"
30 #include "buffer.h"
31 #include "rprintf.h"
32 #include "uart2.h"
33 #include "gps.h"
34  
35 #include "tsip.h"
36  
37 // Program ROM constants
38  
39 // Global variables
40 extern GpsInfoType GpsInfo;
41 #define BUFFERSIZE 0x40
42 u08 TsipPacket[BUFFERSIZE];
43 u08 debug;
44  
45 // function pointer to single byte output routine
46 static void (*TsipTxByteFunc)(unsigned char c);
47  
48 void tsipInit(void (*txbytefunc)(unsigned char c))
49 {
50 // set transmit function
51 // (this function will be used for all SendPacket commands)
52 TsipTxByteFunc = txbytefunc;
53  
54 // set debug status
55 debug = 0;
56  
57 // compose GPS receiver configuration packet
58 u08 packet[4];
59 packet[0] = BV(POS_LLA);
60 packet[1] = BV(VEL_ENU);
61 packet[2] = 0;
62 packet[3] = 0;
63 // send configuration
64 tsipSendPacket(TSIPTYPE_SET_IO_OPTIONS, 4, packet);
65 }
66  
67 void tsipSendPacket(u08 tsipType, u08 dataLength, u08* data)
68 {
69 u08 i;
70 u08 dataIdx = 0;
71  
72 // start of packet
73 TsipPacket[dataIdx++] = DLE;
74 // packet type
75 TsipPacket[dataIdx++] = tsipType;
76 // add packet data
77 for(i=0; i<dataLength; i++)
78 {
79 if(*data == DLE)
80 {
81 // do double-DLE escape sequence
82 TsipPacket[dataIdx++] = *data;
83 TsipPacket[dataIdx++] = *data++;
84 }
85 else
86 TsipPacket[dataIdx++] = *data++;
87 }
88 // end of packet
89 TsipPacket[dataIdx++] = DLE;
90 TsipPacket[dataIdx++] = ETX;
91  
92 for(i=0; i<dataIdx; i++)
93 TsipTxByteFunc(TsipPacket[i]);
94 }
95  
96 u08 tsipProcess(cBuffer* rxBuffer)
97 {
98 u08 foundpacket = FALSE;
99 u08 startFlag = FALSE;
100 u08 data;
101 u08 i,j,k;
102  
103 u08 TsipPacketIdx;
104  
105 // process the receive buffer
106 // go through buffer looking for packets
107 while(rxBuffer->datalength > 1)
108 {
109 // look for a potential start of TSIP packet
110 if(bufferGetAtIndex(rxBuffer,0) == DLE)
111 {
112 // make sure the next byte is not DLE or ETX
113 data = bufferGetAtIndex(rxBuffer,1);
114 if((data != DLE) && (data != ETX))
115 {
116 // found potential start
117 startFlag = TRUE;
118 // done looking for start
119 break;
120 }
121 }
122 else
123 // not DLE, dump character from buffer
124 bufferGetFromFront(rxBuffer);
125 }
126  
127 // if we detected a start, look for end of packet
128 if(startFlag)
129 {
130 for(i=1; i<(rxBuffer->datalength)-1; i++)
131 {
132 // check for potential end of TSIP packet
133 if((bufferGetAtIndex(rxBuffer,i) == DLE) && (bufferGetAtIndex(rxBuffer,i+1) == ETX))
134 {
135 // have a packet end
136 // dump initial DLE
137 bufferGetFromFront(rxBuffer);
138 // copy data to TsipPacket
139 TsipPacketIdx = 0;
140 for(j=0; j<(i-1); j++)
141 {
142 data = bufferGetFromFront(rxBuffer);
143 if(data == DLE)
144 {
145 if(bufferGetAtIndex(rxBuffer,0) == DLE)
146 {
147 // found double-DLE escape sequence, skip one of them
148 bufferGetFromFront(rxBuffer);
149 j++;
150 }
151 }
152 TsipPacket[TsipPacketIdx++] = data;
153 }
154 // dump ending DLE+ETX
155 bufferGetFromFront(rxBuffer);
156 bufferGetFromFront(rxBuffer);
157  
158 // found a packet
159 if(debug)
160 {
161 rprintf("Rx TSIP packet type: 0x%x len: %d rawlen: %d\r\n",
162 TsipPacket[0],
163 TsipPacketIdx,
164 i);
165 for(k=0; k<TsipPacketIdx; k++)
166 {
167 rprintfu08(TsipPacket[k]);
168 rprintfChar(' ');
169 }
170 //rprintfu08(bufferGetFromFront(rxBuffer)); rprintfChar(' ');
171 //rprintfu08(bufferGetFromFront(rxBuffer)); rprintfChar(' ');
172  
173 rprintfCRLF();
174 }
175 // done with this processing session
176 foundpacket = TRUE;
177 break;
178 }
179 }
180 }
181  
182 if(foundpacket)
183 {
184 // switch on the packet type
185 switch(TsipPacket[0])
186 {
187 case TSIPTYPE_GPSTIME: tsipProcessGPSTIME(TsipPacket); break;
188 case TSIPTYPE_POSFIX_XYZ_SP: tsipProcessPOSFIX_XYZ_SP(TsipPacket); break;
189 case TSIPTYPE_VELFIX_XYZ: tsipProcessVELFIX_XYZ(TsipPacket); break;
190  
191 case TSIPTYPE_POSFIX_LLA_SP: tsipProcessPOSFIX_LLA_SP(TsipPacket); break;
192 case TSIPTYPE_VELFIX_ENU: tsipProcessVELFIX_ENU(TsipPacket); break;
193  
194 case TSIPTYPE_RAWDATA: break;
195 default:
196 //if(debug) rprintf("Unhandled TSIP packet type: 0x%x\r\n",TsipPacket[0]);
197 break;
198 }
199 }
200  
201 return foundpacket;
202 }
203  
204 void tsipProcessGPSTIME(u08* packet)
205 {
206 // NOTE: check endian-ness if porting to processors other than the AVR
207 GpsInfo.TimeOfWeek.b[3] = packet[1];
208 GpsInfo.TimeOfWeek.b[2] = packet[2];
209 GpsInfo.TimeOfWeek.b[1] = packet[3];
210 GpsInfo.TimeOfWeek.b[0] = packet[4];
211  
212 GpsInfo.WeekNum = ((u16)packet[5]<<8)|((u16)packet[6]);
213  
214 GpsInfo.UtcOffset.b[3] = packet[7];
215 GpsInfo.UtcOffset.b[2] = packet[8];
216 GpsInfo.UtcOffset.b[1] = packet[9];
217 GpsInfo.UtcOffset.b[0] = packet[10];
218 }
219  
220 void tsipProcessPOSFIX_XYZ_SP(u08* packet)
221 {
222 // NOTE: check endian-ness if porting to processors other than the AVR
223 GpsInfo.PosECEF.x.b[3] = packet[1];
224 GpsInfo.PosECEF.x.b[2] = packet[2];
225 GpsInfo.PosECEF.x.b[1] = packet[3];
226 GpsInfo.PosECEF.x.b[0] = packet[4];
227  
228 GpsInfo.PosECEF.y.b[3] = packet[5];
229 GpsInfo.PosECEF.y.b[2] = packet[6];
230 GpsInfo.PosECEF.y.b[1] = packet[7];
231 GpsInfo.PosECEF.y.b[0] = packet[8];
232  
233 GpsInfo.PosECEF.z.b[3] = packet[9];
234 GpsInfo.PosECEF.z.b[2] = packet[10];
235 GpsInfo.PosECEF.z.b[1] = packet[11];
236 GpsInfo.PosECEF.z.b[0] = packet[12];
237  
238 GpsInfo.PosECEF.TimeOfFix.b[3] = packet[13];
239 GpsInfo.PosECEF.TimeOfFix.b[2] = packet[14];
240 GpsInfo.PosECEF.TimeOfFix.b[1] = packet[15];
241 GpsInfo.PosECEF.TimeOfFix.b[0] = packet[16];
242  
243 GpsInfo.PosECEF.updates++;
244  
245 // GpsInfo.TimeOfFix_ECEF.f = *((float*)&packet[13]);
246 }
247  
248 void tsipProcessVELFIX_XYZ(u08* packet)
249 {
250 }
251  
252 void tsipProcessPOSFIX_LLA_SP(u08* packet)
253 {
254 // NOTE: check endian-ness if porting to processors other than the AVR
255 GpsInfo.PosLLA.lat.b[3] = packet[1];
256 GpsInfo.PosLLA.lat.b[2] = packet[2];
257 GpsInfo.PosLLA.lat.b[1] = packet[3];
258 GpsInfo.PosLLA.lat.b[0] = packet[4];
259  
260 GpsInfo.PosLLA.lon.b[3] = packet[5];
261 GpsInfo.PosLLA.lon.b[2] = packet[6];
262 GpsInfo.PosLLA.lon.b[1] = packet[7];
263 GpsInfo.PosLLA.lon.b[0] = packet[8];
264  
265 GpsInfo.PosLLA.alt.b[3] = packet[9];
266 GpsInfo.PosLLA.alt.b[2] = packet[10];
267 GpsInfo.PosLLA.alt.b[1] = packet[11];
268 GpsInfo.PosLLA.alt.b[0] = packet[12];
269  
270 GpsInfo.PosLLA.TimeOfFix.b[3] = packet[17];
271 GpsInfo.PosLLA.TimeOfFix.b[2] = packet[18];
272 GpsInfo.PosLLA.TimeOfFix.b[1] = packet[18];
273 GpsInfo.PosLLA.TimeOfFix.b[0] = packet[20];
274  
275 GpsInfo.PosLLA.updates++;
276 }
277  
278 void tsipProcessVELFIX_ENU(u08* packet)
279 {
280 // NOTE: check endian-ness if porting to processors other than the AVR
281 GpsInfo.VelENU.east.b[3] = packet[1];
282 GpsInfo.VelENU.east.b[2] = packet[2];
283 GpsInfo.VelENU.east.b[1] = packet[3];
284 GpsInfo.VelENU.east.b[0] = packet[4];
285  
286 GpsInfo.VelENU.north.b[3] = packet[5];
287 GpsInfo.VelENU.north.b[2] = packet[6];
288 GpsInfo.VelENU.north.b[1] = packet[7];
289 GpsInfo.VelENU.north.b[0] = packet[8];
290  
291 GpsInfo.VelENU.up.b[3] = packet[9];
292 GpsInfo.VelENU.up.b[2] = packet[10];
293 GpsInfo.VelENU.up.b[1] = packet[11];
294 GpsInfo.VelENU.up.b[0] = packet[12];
295  
296 GpsInfo.VelENU.TimeOfFix.b[3] = packet[17];
297 GpsInfo.VelENU.TimeOfFix.b[2] = packet[18];
298 GpsInfo.VelENU.TimeOfFix.b[1] = packet[19];
299 GpsInfo.VelENU.TimeOfFix.b[0] = packet[20];
300  
301 GpsInfo.VelENU.updates++;
302 }
303  
304 void tsipProcessRAWDATA(cBuffer* packet)
305 {
306 /*
307 char oft = 1;
308 // process the data in TSIPdata
309 unsigned char SVnum = TSIPdata[oft];
310 unsigned __int32 SNR32 = (TSIPdata[oft+5] << 24) + (TSIPdata[oft+6] << 16) + (TSIPdata[oft+7] << 8) + (TSIPdata[oft+8]);
311 unsigned __int32 codephase32 = (TSIPdata[oft+9] << 24) + (TSIPdata[oft+10] << 16) + (TSIPdata[oft+11] << 8) + (TSIPdata[oft+12]);
312 unsigned __int32 doppler32 = (TSIPdata[oft+13] << 24) + (TSIPdata[oft+14] << 16) + (TSIPdata[oft+15] << 8) + (TSIPdata[oft+16]);
313  
314 unsigned __int64 meastimeH32 = (TSIPdata[oft+17] << 24) | (TSIPdata[oft+18] << 16) | (TSIPdata[oft+19] << 8) | (TSIPdata[oft+20]);
315 unsigned __int64 meastimeL32 = (TSIPdata[oft+21] << 24) | (TSIPdata[oft+22] << 16) | (TSIPdata[oft+23] << 8) | (TSIPdata[oft+24]);
316 unsigned __int64 meastime64 = (meastimeH32 << 32) | (meastimeL32);
317  
318 float SNR = *((float*) &SNR32);
319 float codephase = *((float*) &codephase32);
320 float doppler = *((float*) &doppler32);
321 double meastime = *((double*) &meastime64);
322  
323 // output to screen
324 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);
325 //printf("SV%2d SNR: %5.2f PH: %10.4f DOP: %10.4f TIME: %I64x\n",SVnum,SNR,codephase,doppler,meastime64);
326  
327 // output to file
328 fprintf( logfile, "%2d %5.2f %11.4f %11.4f %5.0I64f %7.2I64f\n",SVnum,SNR,codephase,doppler,meastime,meastime/1.5);
329 */
330 }
331  
{BLAME END}
{FOOTER START}

Powered by WebSVN v2.8.3