0,0 → 1,263 |
/*! \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/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); |
memcpy(&GPSlat,&packet[i],9); |
GPSlat[9]=0; |
// convert to pure degrees [dd.dddd] format |
minutesfrac = modf(GpsInfo.PosLLA.lat.f/100, °rees); |
GpsInfo.PosLLA.lat.f = degrees + (minutesfrac*100)/60; |
// convert to radians |
//!!!KAKL 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); |
memcpy(&GPSlon,&packet[i]+1,9); |
GPSlat[9]=0; |
// convert to pure degrees [dd.dddd] format |
minutesfrac = modf(GpsInfo.PosLLA.lon.f/100, °rees); |
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++; |
} |
|