WebSVN – svnkaklik – //programy/C/avr/gps/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/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++;
}
Subversion Repositories svnkaklik

(root)//programy/C/avr/gps/nmea.c @ 1025 – Rev 507
