/elekIOServ/NMEAParser.c

  1// ============================================
  2// NMEA Parser
  3// Implementation
  4// ============================================
  5
  6// $RCSfile: NMEAParser.c,v $ last changed on $Date: 2006/09/04 13:05:50 $ by $Author: rudolf $
  7
  8// History:
  9//
 10// $Log: NMEAParser.c,v $
 11// Revision 1.5  2006/09/04 13:05:50  rudolf
 12// fix for GCC 4.03
 13//
 14// Revision 1.4  2005/09/21 21:02:48  rudolf
 15// fixed Latitude and Longitude parsing, changed tty to S1
 16//
 17// Revision 1.3  2005/01/31 11:36:31  rudolf
 18// Added parsing of NMEA GPVTG datafield (groundspeed and heading)
 19//
 20// Revision 1.2  2005/01/27 18:16:12  rudolf
 21// changed NMEA parser to work with elekIO serv
 22//
 23// Revision 1.1  2005/01/27 14:59:36  rudolf
 24// added files for GPS receiver connection
 25//
 26//
 27
 28#include <stddef.h>
 29#include <stdlib.h>
 30#include <stdio.h>
 31#include "NMEAParser.h"
 32
 33#define MAXFIELD	25		// maximum field length
 34
 35extern volatile unsigned char ucDataReadyFlag;
 36
 37// ==================
 38// Init NMEA Parser
 39// ==================
 40
 41void NMEAParserInit()
 42{
 43	nState = NP_STATE_SOM;	// Reset Statemachine
 44	dwCommandCount = 0;
 45	Reset();
 46}
 47
 48// ===============================
 49// Process serial input buffer
 50// ===============================
 51
 52unsigned char ParseBuffer(unsigned char *pBuff, int iLen)
 53{
 54
 55	int iIndex = 0;
 56	for(iIndex = 0; iIndex < iLen; iIndex++)
 57	{
 58		ProcessNMEA(pBuff[iIndex]);
 59	}
 60	return TRUE;
 61}
 62
 63// ==================
 64// Process NMEA data
 65// ==================
 66
 67void ProcessNMEA(unsigned char ucData)
 68{
 69	switch(nState)
 70	{
 71		// Search for start of message '$'
 72		case NP_STATE_SOM :
 73			if(ucData == '$')
 74			{
 75				ucChecksum = 0;			// reset checksum
 76				wIndex = 0;				// reset index
 77				nState = NP_STATE_CMD;
 78			}
 79		break;
 80
 81		// Retrieve command (NMEA Address)
 82		case NP_STATE_CMD :
 83			if(ucData != ',' && ucData != '*')
 84			{
 85				pCommand[wIndex++] = ucData;
 86				ucChecksum ^= ucData;
 87
 88				// Check for command overflow
 89				if(wIndex >= NP_MAX_CMD_LEN)
 90				{
 91					nState = NP_STATE_SOM;
 92				}
 93			}
 94			else
 95			{
 96				pCommand[wIndex] = '\0';	// terminate command
 97				ucChecksum ^= ucData;
 98				wIndex = 0;
 99				nState = NP_STATE_DATA;		// goto get data state
100			}
101		break;
102
103		// Store data and check for end of sentence or checksum flag
104		case NP_STATE_DATA :
105			if(ucData == '*') // checksum flag?
106			{
107				pData[wIndex] = '\0';
108				nState = NP_STATE_CHECKSU1;
109			}
110			else // no checksum flag, store data
111			{
112				//
113				// Check for end of sentence with no checksum
114				//
115				if(ucData == '\r')
116				{
117					pData[wIndex] = '\0';
118					ProcessCommand(pCommand, pData);
119					nState = NP_STATE_SOM;
120					return;
121				}
122
123				//
124				// Store data and calculate checksum
125				//
126				ucChecksum ^= ucData;
127				pData[wIndex] = ucData;
128				if(++wIndex >= NP_MAX_DATA_LEN) // Check for buffer overflow
129				{
130					nState = NP_STATE_SOM;
131				}
132			}
133		break;
134
135		// first byte of checksum
136		case NP_STATE_CHECKSU1 :
137			if( (ucData - '0') <= 9)
138			{
139				ucReceivedChecksum = (ucData - '0') << 4;
140			}
141			else
142			{
143				ucReceivedChecksum = (ucData - 'A' + 10) << 4;
144			}
145
146			nState = NP_STATE_CHECKSU2;
147
148		break;
149
150		// second byte of checksum
151		case NP_STATE_CHECKSU2 :
152			if( (ucData - '0') <= 9)
153			{
154				ucReceivedChecksum |= (ucData - '0');
155			}
156			else
157			{
158				ucReceivedChecksum |= (ucData - 'A' + 10);
159			}
160
161			if(ucChecksum == ucReceivedChecksum)
162			{
163				ProcessCommand(pCommand, pData);
164			}
165
166			nState = NP_STATE_SOM;
167		break;
168
169		default : nState = NP_STATE_SOM;
170	}
171}
172
173//=======================================================================
174// Process NMEA sentence - Use the NMEA address (*pCommand) and call the
175// appropriate sentense data processor.
176//=======================================================================
177
178unsigned char ProcessCommand(unsigned char *pCommand, unsigned char *pData)
179{
180	//
181	// GPGGA
182	//
183	if( strcmp((char *)pCommand, "GPGGA") == 0)
184	{
185		ProcessGPGGA(pData);
186	}
187
188	//
189	// GPVTG
190	//
191	if( strcmp((char *)pCommand, "GPVTG") == 0)
192	{
193		ProcessGPVTG(pData);
194	}
195
196	dwCommandCount++;
197	return TRUE;
198}
199
200//===========================================================================
201// Name:		GetField
202//
203// Description:	This function will get the specified field in a NMEA string.
204//
205// Entry:		unsigned char *pData -		Pointer to NMEA string
206//				unsigned char *pField -		pointer to returned field
207//				int nfieldNum -		Field offset to get
208//				int nMaxFieldLen -	Maximum of unsigned chars pFiled can handle
209//===========================================================================
210
211unsigned char GetField(unsigned char *pData, unsigned char *pField, int nFieldNum, int nMaxFieldLen)
212{
213	//
214	// Validate params
215	//
216	if(pData == NULL || pField == NULL || nMaxFieldLen <= 0)
217	{
218		return FALSE;
219	}
220
221	//
222	// Go to the beginning of the selected field
223	//
224	int i = 0;
225	int nField = 0;
226	while(nField != nFieldNum && pData[i])
227	{
228		if(pData[i] == ',')
229		{
230			nField++;
231		}
232
233		i++;
234
235		if(pData[i] == 0)
236		{
237			pField[0] = '\0';
238			return FALSE;
239		}
240	}
241
242	if(pData[i] == ',' || pData[i] == '*')
243	{
244		pField[0] = '\0';
245		return FALSE;
246	}
247
248	//
249	// copy field from pData to Field
250	//
251	int i2 = 0;
252	while(pData[i] != ',' && pData[i] != '*' && pData[i])
253	{
254		pField[i2] = pData[i];
255		i2++; i++;
256
257		//
258		// check if field is too big to fit on passed parameter. If it is,
259		// crop returned field to its max length.
260		//
261		if(i2 >= nMaxFieldLen)
262		{
263			i2 = nMaxFieldLen-1;
264			break;
265		}
266	}
267	pField[i2] = '\0';
268
269	return TRUE;
270}
271
272//===========================================================================
273// Reset: Reset all NMEA data to start-up default values.
274//===========================================================================
275
276void Reset()
277{
278	int i;
279
280	//
281	// GPGGA Data
282	//
283	ucGGAHour = 0;				//
284	ucGGAMinute = 0;			//
285	ucGGASecond = 0;			//
286	dGGALatitude = 0.0;			// < 0 = South, > 0 = North
287	dGGALongitude = 0.0;		// < 0 = West, > 0 = East
288	ucGGAGPSQuality = 0;		// 0 = fix not available,
289								// 1 = GPS sps mode,
290								// 2 = Differential GPS, SPS mode, fix valid,
291								// 3 = GPS PPS mode, fix valid
292	ucGGANumOfSatsInUse = 0;	//
293	dGGAHDOP = 0.0;				//
294	dGGAAltitude = 0.0;			// Altitude: mean-sea-level (geoid) meters
295	dwGGACount = 0;				//
296	nGGAOldVSpeedSeconds = 0;	//
297	dGGAOldVSpeedAlt = 0.0;		//
298	dGGAVertSpeed = 0.0;		//
299}
300
301//===========================================================================
302// ProcessGPGGA: extract time, date, postition and quality from GPGGA data
303//===========================================================================
304
305void ProcessGPGGA(unsigned char *pData)
306{
307	unsigned char pField[MAXFIELD];
308	unsigned char pBuff[10];
309
310	//
311	// Time
312	//
313	if(GetField(pData, pField, 0, MAXFIELD))
314	{
315		// Hour
316		pBuff[0] = pField[0];
317		pBuff[1] = pField[1];
318		pBuff[2] = '\0';
319		ucGGAHour = atoi((char*)pBuff);
320
321		// minute
322		pBuff[0] = pField[2];
323		pBuff[1] = pField[3];
324		pBuff[2] = '\0';
325		ucGGAMinute = atoi((char*)pBuff);
326
327		// Second
328		pBuff[0] = pField[4];
329		pBuff[1] = pField[5];
330		pBuff[2] = '\0';
331		ucGGASecond = atoi((char*)pBuff);
332	}
333
334	//
335	// Latitude
336	//
337	if(GetField(pData, pField, 1, MAXFIELD))
338	{
339		dGGALatitude = atof((char*)pField);
340	}
341	if(GetField(pData, pField, 2, MAXFIELD))
342	{
343		if(pField[0] == 'S')
344		{
345			dGGALatitude = -dGGALatitude;
346		}
347	}
348
349	//
350	// Longitude
351	//
352	if(GetField(pData, pField, 3, MAXFIELD))
353	{
354		dGGALongitude = atof((char *)pField);
355	}
356	if(GetField(pData, pField, 4, MAXFIELD))
357	{
358		if(pField[0] == 'W')
359		{
360			dGGALongitude = -dGGALongitude;
361		}
362	}
363
364	//
365	// GPS quality
366	//
367	if(GetField(pData, pField, 5, MAXFIELD))
368	{
369		ucGGAGPSQuality = pField[0] - '0';
370	}
371
372	//
373	// Satellites in use
374	//
375	if(GetField(pData, pField, 6, MAXFIELD))
376	{
377		pBuff[0] = pField[0];
378		pBuff[1] = pField[1];
379		pBuff[2] = '\0';
380		ucGGANumOfSatsInUse = atoi((char*)pBuff);
381	}
382
383	//
384	// HDOP
385	//
386	if(GetField(pData, pField, 7, MAXFIELD))
387	{
388		dGGAHDOP = atof((char *)pField);
389	}
390
391	//
392	// Altitude
393	//
394	if(GetField(pData, pField, 8, MAXFIELD))
395	{
396		dGGAAltitude = atof((char *)pField);
397	}
398
399	//
400	// Durive vertical speed (bonus)
401	//
402	int nSeconds = (int)ucGGAMinute * 60 + (int)ucGGASecond;
403	if(nSeconds > nGGAOldVSpeedSeconds)
404	{
405		double dDiff = (double)(nGGAOldVSpeedSeconds-nSeconds);
406		double dVal = dDiff/60.0;
407		if(dVal != 0.0)
408		{
409			dGGAVertSpeed = (dGGAOldVSpeedAlt - dGGAAltitude) / dVal;
410		}
411	}
412	dGGAOldVSpeedAlt = dGGAAltitude;
413	nGGAOldVSpeedSeconds = nSeconds;
414
415	if(ucGGAGPSQuality > 0)		// position fixes are valid
416		ucDataReadyFlag = TRUE;
417	dwGGACount++;
418}
419
420
421//===========================================================================
422// ProcessGPVTG: extract speed and heading
423//===========================================================================
424
425void ProcessGPVTG(unsigned char *pData)
426{
427   unsigned char pField[MAXFIELD];
428   unsigned char pBuff[10];
429   if(GetField(pData, pField, 0, MAXFIELD))
430   {
431      // True Heading
432
433      pBuff[0] = pField[0];
434      pBuff[1] = pField[1];
435      pBuff[2] = pField[2];
436      pBuff[3] = pField[3];
437      pBuff[4] = pField[4];
438      pBuff[5] = '\0';
439
440      dVTGTrueHeading = atof((char*)pBuff);
441   }
442
443   if(GetField(pData, pField, 2, MAXFIELD))
444   {
445      // Magnetic Heading
446
447      pBuff[0] = pField[0];
448      pBuff[1] = pField[1];
449      pBuff[2] = pField[2];
450      pBuff[3] = pField[3];
451      pBuff[4] = pField[4];
452      pBuff[5] = '\0';
453
454      dVTGMagneticHeading = atof((char*)pBuff);
455   }
456
457   if(GetField(pData, pField, 4, MAXFIELD))
458   {
459      // Groundspeed in knots
460
461      pBuff[0] = pField[0];
462      pBuff[1] = pField[1];
463      pBuff[2] = pField[2];
464      pBuff[3] = pField[3];
465      pBuff[4] = pField[4];
466      pBuff[5] = '\0';
467
468      dVTGSpeedInknots = atof((char*)pBuff);
469   }
470
471   if(GetField(pData, pField, 6, MAXFIELD))
472   {
473      // Groundspeed in kmh
474
475      pBuff[0] = pField[0];
476      pBuff[1] = pField[1];
477      pBuff[2] = pField[2];
478      pBuff[3] = pField[3];
479      pBuff[4] = pField[4];
480      pBuff[5] = '\0';
481
482      dVTGSpeedInKmh = atof((char*)pBuff);
483   }
484
485   dwVTGCount++;
486}