?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 .c \brief Mitel GPS STX/ETX driver function library. */
2 //*****************************************************************************
3 //
4 // File Name : 'mitelgps.c'
5 // Title : Mitel GPS STX/ETX driver function library
6 // Author : Pascal Stang - Copyright (C) 2002
7 // Created : 2003.04.11
8 // Revised : 2003.08.26
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 <string.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 "mitelgps.h"
36  
37 // Program ROM constants
38  
39 // Global variables
40 // external GPS information structure/repository (in gps.h/gps.c)
41 extern GpsInfoType GpsInfo;
42 // packet processing buffer
43 u08 MitelGpsPacket[MITELGPS_BUFFERSIZE];
44 // debug flag
45 u08 debug;
46 #define MITELGPS_DEBUG_PKTPARSE 0x01
47 #define MITELGPS_DEBUG_EXTRACT 0x02
48 // function pointer to single byte output routine
49 static void (*TxByteFunc)(unsigned char c);
50  
51 void mitelgpsInit(void (*txbytefunc)(unsigned char c))
52 {
53 // set transmit function
54 // (this function will be used for all SendPacket commands)
55 TxByteFunc = txbytefunc;
56 // set debug status
57 debug = 0;
58 }
59  
60 void mitelgpsSendPacket(u08* data, u08 dataLength)
61 {
62 u08 i;
63 u08 dataIdx = 0;
64 u08 checksum = 0;
65  
66 // start of packet
67 MitelGpsPacket[dataIdx++] = STX;
68 // add packet type and packet data
69 for(i=0; i<dataLength; i++)
70 {
71 checksum ^= MitelGpsPacket[dataIdx];
72 MitelGpsPacket[dataIdx++] = *data++;
73 }
74 // checksum
75 convertIntToAsciiHex(checksum, &MitelGpsPacket[dataIdx], 2);
76 dataIdx += 2;
77 // end of packet
78 MitelGpsPacket[dataIdx++] = ETX;
79  
80 // send it
81 for(i=0; i<dataIdx; i++)
82 TxByteFunc(MitelGpsPacket[i]);
83 }
84  
85 u08 mitelgpsProcess(cBuffer* rxBuffer)
86 {
87 u08 foundpacket = FALSE;
88 u08 startFlag = FALSE;
89 u08 checksum = 0;
90 u08 packetType;
91 u16 i,j;
92  
93 // process the receive buffer
94 // go through buffer looking for packets
95 while(rxBuffer->datalength > 1)
96 {
97 // look for a start of Mitel GPS STX/ETX packet
98 if(bufferGetAtIndex(rxBuffer,0) == STX)
99 {
100 // found start
101 startFlag = TRUE;
102 // done looking for start
103 break;
104 }
105 else
106 // not STX, dump character from buffer
107 bufferGetFromFront(rxBuffer);
108 }
109  
110 // if we detected a start, look for end of packet
111 if(startFlag)
112 {
113 for(i=1; i<(rxBuffer->datalength); i++)
114 {
115 // check for end of Mitel GPS STX/ETX packet
116 if(bufferGetAtIndex(rxBuffer,i) == ETX)
117 {
118 // have a packet end
119 // dump initial STX
120 bufferGetFromFront(rxBuffer);
121 // copy data to MitelGpsPacket
122 for(j=0; j<(i-1); j++)
123 {
124 MitelGpsPacket[j] = bufferGetFromFront(rxBuffer);
125 checksum ^= MitelGpsPacket[j];
126 }
127 // null-terminate copied string
128 MitelGpsPacket[j] = 0;
129 // dump ending ETX
130 bufferGetFromFront(rxBuffer);
131  
132 // verify checksum
133 // undo checksum summing of the checksum itself
134 checksum ^= MitelGpsPacket[j-2];
135 checksum ^= MitelGpsPacket[j-1];
136 if( checksum == convertAsciiHexToInt(&MitelGpsPacket[j-2], 2) )
137 {
138 // found a good packet
139 if(debug & MITELGPS_DEBUG_PKTPARSE)
140 {
141 rprintf("Rx Mitel GPS packet type: %c%c%c len: %d\r\n",
142 MitelGpsPacket[0], MitelGpsPacket[1], MitelGpsPacket[2], j);
143 rprintfStr(MitelGpsPacket);
144 rprintfCRLF();
145 }
146 // done with this processing session
147 foundpacket = TRUE;
148 break;
149 }
150 else
151 {
152 if(debug & MITELGPS_DEBUG_PKTPARSE)
153 {
154 rprintf("Rx Mitel GPS packet type: %c%c%c len: %d Bad Checksum Rcvd: 0x%c%c Calc: 0x%x\r\n",
155 MitelGpsPacket[0], MitelGpsPacket[1], MitelGpsPacket[2], j, MitelGpsPacket[j-2], MitelGpsPacket[j-1], checksum);
156 }
157 }
158 }
159 }
160 }
161  
162 // handle and direct the received packet
163 if(foundpacket)
164 {
165 // switch on the packet type
166 packetType = convertAsciiHexToInt(&MitelGpsPacket[1], 2);
167 switch( packetType )
168 {
169 case MITELTYPE_NAVDATAGND: mitelgpsProcessNAVDATAGND(MitelGpsPacket); break;
170 case MITELTYPE_CHNLSTATGND: mitelgpsProcessCHNLSTATGND(MitelGpsPacket); break;
171 case MITELTYPE_NAVDATA: mitelgpsProcessNAVDATA(MitelGpsPacket); break;
172 case MITELTYPE_RAWDATA: mitelgpsProcessRAWDATA(MitelGpsPacket); break;
173 case MITELTYPE_CHNLSTAT: mitelgpsProcessCHNLSTAT(MitelGpsPacket); break;
174 case MITELTYPE_RELNAVECEF: break;
175 case MITELTYPE_RELNAVRTN: break;
176 default:
177 if(debug & MITELGPS_DEBUG_PKTPARSE)
178 rprintf("Unhandled Mitel GPS packet type: 0x%x\r\n", packetType);
179 break;
180 }
181 }
182  
183 return foundpacket;
184 }
185  
186 void mitelgpsProcessNAVDATAGND(u08* packet)
187 {
188 // process "F00" report packets - Navigation Data (Ground)
189 char* endptr;
190  
191 if(debug & MITELGPS_DEBUG_EXTRACT)
192 {
193 rprintf("MITELGPS: ");
194 rprintfStr(packet);
195 rprintfCRLF();
196 }
197  
198 // start parsing just after "F00"
199 // get latitude [sdd.dddddd]
200 GpsInfo.PosLLA.lat.f = strtod(&packet[3], &endptr);
201 // get longitude [sddd.dddddd]
202 GpsInfo.PosLLA.lon.f = strtod(&packet[3+10], &endptr);
203 // get altitude [sxxxxxx.x]
204 GpsInfo.PosLLA.alt.f = strtod(&packet[3+10+11], &endptr);
205 // get speed [sxxx.xx]
206 GpsInfo.VelHS.speed.f = strtod(&packet[3+10+11+9], &endptr);
207 // get heading [ddd]
208 GpsInfo.VelHS.heading.f = strtod(&packet[3+10+11+9+7], &endptr);
209  
210 // get # of SVs tracked [xx]
211 GpsInfo.numSVs = atoi(&packet[3+10+11+9+7+5+7+5+5+5]);
212 }
213  
214 void mitelgpsProcessCHNLSTATGND(u08* packet)
215 {
216 // process "F03" report packets - Channel Status (Ground)
217 }
218  
219 void mitelgpsProcessNAVDATA(u08* packet)
220 {
221 // process "F40" report packets - Navigation Data
222 char* endptr;
223  
224 // start parsing just after "F40"
225 // get gps week number [xxxx]=4
226 GpsInfo.WeekNum = atoi(&packet[3]);
227 // get gps time of week [xxxxxx.xxxxx]=12
228 GpsInfo.TimeOfWeek.f = strtod(&packet[3+4], &endptr);
229 // gps-utc time difference? [xx]=2
230 // get ECEF X [sxxxxxxxx.xx]=12
231 GpsInfo.PosECEF.x.f = strtod(&packet[3+4+12+2], &endptr);
232 // get ECEF Y [sxxxxxxxx.xx]=12
233 GpsInfo.PosECEF.y.f = strtod(&packet[3+4+12+2+12], &endptr);
234 // get ECEF Z [sxxxxxxxx.xx]=12
235 GpsInfo.PosECEF.z.f = strtod(&packet[3+4+12+2+12+12], &endptr);
236 // get ECEF vX [sxxxxxxxx.xx]=12
237 GpsInfo.VelECEF.x.f = strtod(&packet[3+4+12+2+12+12+12], &endptr);
238 // get ECEF vY [sxxxxxxxx.xx]=12
239 GpsInfo.VelECEF.y.f = strtod(&packet[3+4+12+2+12+12+12+12], &endptr);
240 // get ECEF vZ [sxxxxxxxx.xx]=12
241 GpsInfo.VelECEF.z.f = strtod(&packet[3+4+12+2+12+12+12+12+12], &endptr);
242 }
243  
244 void mitelgpsProcessRAWDATA(u08* packet)
245 {
246 // process "F42" report packets - Pseudorange, carrier phase, doppler
247 }
248  
249 void mitelgpsProcessCHNLSTAT(u08* packet)
250 {
251 // process "F43" report packets - Channel Status
252 }
253  
254 // data conversions
255 u32 convertAsciiHexToInt(u08* string, u08 numdigits)
256 {
257 u08 i;
258 u32 num = 0;
259  
260 for(i=0; i<numdigits; i++)
261 {
262 // shift number up
263 num = num<<4;
264 // decode hex digit
265 if(string[i] >= 'a')
266 num |= string[i]-'a'+10;
267 else if(string[i] >= 'A')
268 num |= string[i]-'A'+10;
269 else
270 num |= string[i]-'0';
271 }
272 return num;
273 }
274  
275 void convertIntToAsciiHex(u32 num, u08* string, u08 numdigits)
276 {
277 u08 i;
278  
279 for(i=0; i<numdigits; i++)
280 {
281 if((num & 0x0000000F) < 10)
282 string[numdigits-1-i] = (num & 0x0000000F)+'0';
283 else
284 string[numdigits-1-i] = (num & 0x0000000F)+'A'-10;
285 // next digit
286 num = num>>4;
287 }
288 }
{BLAME END}
{FOOTER START}

Powered by WebSVN v2.8.3