Arduino/build/shared/lib/avrlib/nmea.c

/*! \file nmea.c \brief NMEA protocol function library. */
//*****************************************************************************
//
// File Name	: 'nmea.c'
// Title		: NMEA protocol function library
// Author		: Pascal Stang - Copyright (C) 2002
// Created		: 2002.08.27
// Revised		: 2002.08.27
// 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/signal.h>
	#include <avr/interrupt.h>
	#include <avr/pgmspace.h>
#endif
#include <string.h>
#include <stdlib.h>
#include <math.h>

#include "global.h"
#include "buffer.h"
#include "rprintf.h"
#include "gps.h"

#include "nmea.h"

// Program ROM constants

// Global variables
extern GpsInfoType GpsInfo;
u08 NmeaPacket[NMEA_BUFFERSIZE];

void nmeaInit(void)
{
}

u08* nmeaGetPacketBuffer(void)
{
	return NmeaPacket;
}

u08 nmeaProcess(cBuffer* rxBuffer)
{
	u08 foundpacket = NMEA_NODATA;
	u08 startFlag = FALSE;
	//u08 data;
	u16 i,j;

	// process the receive buffer
	// go through buffer looking for packets
	while(rxBuffer->datalength)
	{
		// look for a start of NMEA packet
		if(bufferGetAtIndex(rxBuffer,0) == '$')
		{
			// found start
			startFlag = TRUE;
			// when start is found, we leave it intact in the receive buffer
			// in case the full NMEA string is not completely received.  The
			// start will be detected in the next nmeaProcess iteration.

			// done looking for start
			break;
		}
		else
			bufferGetFromFront(rxBuffer);
	}
	
	// if we detected a start, look for end of packet
	if(startFlag)
	{
		for(i=1; i<(rxBuffer->datalength)-1; i++)
		{
			// check for end of NMEA packet <CR><LF>
			if((bufferGetAtIndex(rxBuffer,i) == '\r') && (bufferGetAtIndex(rxBuffer,i+1) == '\n'))
			{
				// have a packet end
				// dump initial '$'
				bufferGetFromFront(rxBuffer);
				// copy packet to NmeaPacket
				for(j=0; j<(i-1); j++)
				{
					// although NMEA strings should be 80 characters or less,
					// receive buffer errors can generate erroneous packets.
					// Protect against packet buffer overflow
					if(j<(NMEA_BUFFERSIZE-1))
						NmeaPacket[j] = bufferGetFromFront(rxBuffer);
					else
						bufferGetFromFront(rxBuffer);
				}
				// null terminate it
				NmeaPacket[j] = 0;
				// dump <CR><LF> from rxBuffer
				bufferGetFromFront(rxBuffer);
				bufferGetFromFront(rxBuffer);

				#ifdef NMEA_DEBUG_PKT
				rprintf("Rx NMEA packet type: ");
				rprintfStrLen(NmeaPacket, 0, 5);
				rprintfStrLen(NmeaPacket, 5, (i-1)-5);
				rprintfCRLF();
				#endif
				// found a packet
				// done with this processing session
				foundpacket = NMEA_UNKNOWN;
				break;
			}
		}
	}

	if(foundpacket)
	{
		// check message type and process appropriately
		if(!strncmp(NmeaPacket, "GPGGA", 5))
		{
			// process packet of this type
			nmeaProcessGPGGA(NmeaPacket);
			// report packet type
			foundpacket = NMEA_GPGGA;
		}
		else if(!strncmp(NmeaPacket, "GPVTG", 5))
		{
			// process packet of this type
			nmeaProcessGPVTG(NmeaPacket);
			// report packet type
			foundpacket = NMEA_GPVTG;
		}
	}
	else if(rxBuffer->datalength >= rxBuffer->size)
	{
		// if we found no packet, and the buffer is full
		// we're logjammed, flush entire buffer
		bufferFlush(rxBuffer);
	}
	return foundpacket;
}

void nmeaProcessGPGGA(u08* packet)
{
	u08 i;
	char* endptr;
	double degrees, minutesfrac;

	#ifdef NMEA_DEBUG_GGA
	rprintf("NMEA: ");
	rprintfStr(packet);
	rprintfCRLF();
	#endif

	// start parsing just after "GPGGA,"
	i = 6;
	// attempt to reject empty packets right away
	if(packet[i]==',' && packet[i+1]==',')
		return;

	// get UTC time [hhmmss.sss]
	GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], &endptr);
	while(packet[i++] != ',');				// next field: latitude
	
	// get latitude [ddmm.mmmmm]
	GpsInfo.PosLLA.lat.f = strtod(&packet[i], &endptr);
	// convert to pure degrees [dd.dddd] format
	minutesfrac = modf(GpsInfo.PosLLA.lat.f/100, &degrees);
	GpsInfo.PosLLA.lat.f = degrees + (minutesfrac*100)/60;
	// convert to radians
	GpsInfo.PosLLA.lat.f *= (M_PI/180);
	while(packet[i++] != ',');				// next field: N/S indicator
	
	// correct latitute for N/S
	if(packet[i] == 'S') GpsInfo.PosLLA.lat.f = -GpsInfo.PosLLA.lat.f;
	while(packet[i++] != ',');				// next field: longitude
	
	// get longitude [ddmm.mmmmm]
	GpsInfo.PosLLA.lon.f = strtod(&packet[i], &endptr);
	// convert to pure degrees [dd.dddd] format
	minutesfrac = modf(GpsInfo.PosLLA.lon.f/100, &degrees);
	GpsInfo.PosLLA.lon.f = degrees + (minutesfrac*100)/60;
	// convert to radians
	GpsInfo.PosLLA.lon.f *= (M_PI/180);
	while(packet[i++] != ',');				// next field: E/W indicator

	// correct latitute for E/W
	if(packet[i] == 'W') GpsInfo.PosLLA.lon.f = -GpsInfo.PosLLA.lon.f;
	while(packet[i++] != ',');				// next field: position fix status

	// position fix status
	// 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
	// check for good position fix
	if( (packet[i] != '0') && (packet[i] != ',') )
		GpsInfo.PosLLA.updates++;
	while(packet[i++] != ',');				// next field: satellites used
	
	// get number of satellites used in GPS solution
	GpsInfo.numSVs = atoi(&packet[i]);
	while(packet[i++] != ',');				// next field: HDOP (horizontal dilution of precision)
	while(packet[i++] != ',');				// next field: altitude
	
	// get altitude (in meters)
	GpsInfo.PosLLA.alt.f = strtod(&packet[i], &endptr);

	while(packet[i++] != ',');				// next field: altitude units, always 'M'
	while(packet[i++] != ',');				// next field: geoid seperation
	while(packet[i++] != ',');				// next field: seperation units
	while(packet[i++] != ',');				// next field: DGPS age
	while(packet[i++] != ',');				// next field: DGPS station ID
	while(packet[i++] != '*');				// next field: checksum
}

void nmeaProcessGPVTG(u08* packet)
{
	u08 i;
	char* endptr;

	#ifdef NMEA_DEBUG_VTG
	rprintf("NMEA: ");
	rprintfStr(packet);
	rprintfCRLF();
	#endif

	// start parsing just after "GPVTG,"
	i = 6;
	// attempt to reject empty packets right away
	if(packet[i]==',' && packet[i+1]==',')
		return;

	// get course (true north ref) in degrees [ddd.dd]
	GpsInfo.VelHS.heading.f = strtod(&packet[i], &endptr);
	while(packet[i++] != ',');				// next field: 'T'
	while(packet[i++] != ',');				// next field: course (magnetic north)

	// get course (magnetic north ref) in degrees [ddd.dd]
	//GpsInfo.VelHS.heading.f = strtod(&packet[i], &endptr);
	while(packet[i++] != ',');				// next field: 'M'
	while(packet[i++] != ',');				// next field: speed (knots)

	// get speed in knots
	//GpsInfo.VelHS.speed.f = strtod(&packet[i], &endptr);
	while(packet[i++] != ',');				// next field: 'N'
	while(packet[i++] != ',');				// next field: speed (km/h)

	// get speed in km/h
	GpsInfo.VelHS.speed.f = strtod(&packet[i], &endptr);
	while(packet[i++] != ',');				// next field: 'K'
	while(packet[i++] != '*');				// next field: checksum

	GpsInfo.VelHS.updates++;
}