Ugly gps code for GCS, Lat,lon,alt,SVs currently parsed

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1220 ebee16cc-31ac-478f-84a7-5cbb03baadba

ground/src/plugins/gpsdisplay/buffer.cpp

@@ -0,0 +1,139 @@
+/**
+ ******************************************************************************
+ *
+ * @file       buffer.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      see below
+ * 	       As with all modules only the initialize function is exposed all other
+ * 	       interactions with the module take place through the event queue and
+ *             objects.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/*! \file buffer.c \brief Multipurpose byte buffer structure and methods. */
+//*****************************************************************************
+//
+// File Name	: 'buffer.c'
+// Title		: Multipurpose byte buffer structure and methods
+// Author		: Pascal Stang - Copyright (C) 2001-2002
+// Created		: 9/23/2001
+// Revised		: 9/23/2001
+// Version		: 1.0
+// Target MCU	: any
+// Editor Tabs	: 4
+//
+// This code is distributed under the GNU Public License
+//		which can be found at http://www.gnu.org/licenses/gpl.txt
+//
+//*****************************************************************************
+
+#include "buffer.h"
+
+// global variables
+
+// initialization
+
+void bufferInit(cBuffer* buffer, unsigned char *start, unsigned short size)
+{
+	// set start pointer of the buffer
+	buffer->dataptr = start;
+	buffer->size = size;
+	// initialize index and length
+	buffer->dataindex = 0;
+	buffer->datalength = 0;
+}
+
+// access routines
+unsigned char  bufferGetFromFront(cBuffer* buffer)
+{
+	unsigned char data = 0;
+
+	// check to see if there's data in the buffer
+	if(buffer->datalength)
+	{
+		// get the first character from buffer
+		data = buffer->dataptr[buffer->dataindex];
+		// move index down and decrement length
+		buffer->dataindex++;
+		if(buffer->dataindex >= buffer->size)
+		{
+			buffer->dataindex %= buffer->size;
+		}
+		buffer->datalength--;
+	}
+	// return
+	return data;
+}
+
+void bufferDumpFromFront(cBuffer* buffer, unsigned short numbytes)
+{
+	// dump numbytes from the front of the buffer
+	// are we dumping less than the entire buffer?
+	if(numbytes < buffer->datalength)
+	{
+		// move index down by numbytes and decrement length by numbytes
+		buffer->dataindex += numbytes;
+		if(buffer->dataindex >= buffer->size)
+		{
+			buffer->dataindex %= buffer->size;
+		}
+		buffer->datalength -= numbytes;
+	}
+	else
+	{
+		// flush the whole buffer
+		buffer->datalength = 0;
+	}
+}
+
+unsigned char bufferGetAtIndex(cBuffer* buffer, unsigned short index)
+{
+	// return character at index in buffer
+	return buffer->dataptr[(buffer->dataindex+index)%(buffer->size)];
+}
+
+unsigned char bufferAddToEnd(cBuffer* buffer, unsigned char data)
+{
+	// make sure the buffer has room
+	if(buffer->datalength < buffer->size)
+	{
+		// save data byte at end of buffer
+		buffer->dataptr[(buffer->dataindex + buffer->datalength) % buffer->size] = data;
+		// increment the length
+		buffer->datalength++;
+		// return success
+		return -1;
+	}
+	else return 0;
+}
+
+unsigned char bufferIsNotFull(cBuffer* buffer)
+{
+	// check to see if the buffer has room
+	// return true if there is room
+	return (buffer->datalength < buffer->size);
+}
+
+void bufferFlush(cBuffer* buffer)
+{
+	// flush contents of the buffer
+	buffer->datalength = 0;
+}
+

ground/src/plugins/gpsdisplay/buffer.h

@@ -0,0 +1,87 @@
+/**
+ ******************************************************************************
+ *
+ * @file       buffer.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      see below
+ * 	       As with all modules only the initialize function is exposed all other
+ * 	       interactions with the module take place through the event queue and
+ *             objects.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+
+/*! \file buffer.h \brief Multipurpose byte buffer structure and methods. */
+//*****************************************************************************
+//
+// File Name	: 'buffer.h'
+// Title		: Multipurpose byte buffer structure and methods
+// Author		: Pascal Stang - Copyright (C) 2001-2002
+// Created		: 9/23/2001
+// Revised		: 11/16/2002
+// Version		: 1.1
+// Target MCU	: any
+// Editor Tabs	: 4
+//
+///	\ingroup general
+/// \defgroup buffer Circular Byte-Buffer Structure and Function Library (buffer.c)
+/// \code #include "buffer.h" \endcode
+/// \par Overview
+///		This byte-buffer structure provides an easy and efficient way to store
+///		and process a stream of bytes.  You can create as many buffers as you
+///		like (within memory limits), and then use this common set of functions to
+///		access each buffer.  The buffers are designed for FIFO operation (first
+///		in, first out).  This means that the first byte you put in the buffer
+///		will be the first one you get when you read out the buffer.  Supported
+///		functions include buffer initialize, get byte from front of buffer, add
+///		byte to end of buffer, check if buffer is full, and flush buffer.  The
+///		buffer uses a circular design so no copying of data is ever necessary.
+///		This buffer is not dynamically allocated, it has a user-defined fixed
+///		maximum size.  This buffer is used in many places in the avrlib code.
+//
+// This code is distributed under the GNU Public License
+//		which can be found at http://www.gnu.org/licenses/gpl.txt
+//
+//*****************************************************************************
+//@{
+
+#ifndef BUFFER_HPP
+#define BUFFER_HPP
+
+// structure/typdefs
+
+//! cBuffer structure
+typedef struct struct_cBuffer
+{
+	unsigned char *dataptr;			///< the physical memory address where the buffer is stored
+	unsigned short size;			///< the allocated size of the buffer
+	unsigned short datalength;		///< the length of the data currently in the buffer
+	unsigned short dataindex;		///< the index into the buffer where the data starts
+} cBuffer;
+
+// function prototypes
+
+//! initialize a buffer to start at a given address and have given size
+void			bufferInit(cBuffer* buffer, unsigned char *start, unsigned short size);
+
+//! get the first byte from the front of the buffer
+unsigned char	bufferGetFromFront(cBuffer* buffer);
+
+//! dump (discard) the first numbytes from the front of the buffer
+void bufferDumpFromFront(cBuffer* buffer, unsigned short numbytes);
+
+//! get a byte at the specified index in the buffer (kind of like array access)
+// ** note: this does not remove the byte that was read from the buffer
+unsigned char	bufferGetAtIndex(cBuffer* buffer, unsigned short index);
+
+//! add a byte to the end of the buffer
+unsigned char	bufferAddToEnd(cBuffer* buffer, unsigned char data);
+
+//! check if the buffer is full/not full (returns non-zero value if not full)
+unsigned char	bufferIsNotFull(cBuffer* buffer);
+
+//! flush (clear) the contents of the buffer
+void			bufferFlush(cBuffer* buffer);
+
+#endif
+//@}

ground/src/plugins/gpsdisplay/gpsdisplay.pro

@@ -5,13 +5,15 @@ include(../../openpilotgcsplugin.pri)
 include(../../plugins/coreplugin/coreplugin.pri)
 include(gpsdisplay_dependencies.pri)
 include(../../libs/qwt/qwt.pri)
-HEADERS += gpsdisplayplugin.h
+HEADERS += gpsdisplayplugin.h \
+    buffer.h
 HEADERS += gpsdisplaygadget.h
 HEADERS += gpsdisplaywidget.h
 HEADERS += gpsdisplaygadgetfactory.h
 HEADERS += gpsdisplaygadgetconfiguration.h
 HEADERS += gpsdisplaygadgetoptionspage.h
-SOURCES += gpsdisplayplugin.cpp
+SOURCES += gpsdisplayplugin.cpp \
+    buffer.cpp
 SOURCES += gpsdisplaygadget.cpp
 SOURCES += gpsdisplaygadgetfactory.cpp
 SOURCES += gpsdisplaywidget.cpp

ground/src/plugins/gpsdisplay/gpsdisplaywidget.cpp

@@ -35,12 +35,71 @@
 #include <QDebug>
 #include <QThread>
 
+// constants/macros/typdefs
+#define NMEA_BUFFERSIZE		128
+
+// Message Codes
+#define NMEA_NODATA		0	// No data. Packet not available, bad, or not decoded
+#define NMEA_GPGGA		1	// Global Positioning System Fix Data
+#define NMEA_GPVTG		2	// Course over ground and ground speed
+#define NMEA_GPGLL		3	// Geographic position - latitude/longitude
+#define NMEA_GPGSV		4	// GPS satellites in view
+#define NMEA_GPGSA		5	// GPS DOP and active satellites
+#define NMEA_GPRMC		6	// Recommended minimum specific GPS data
+#define NMEA_UNKNOWN	0xFF// Packet received but not known
+
+#define GPS_TIMEOUT_MS 500
+
+// Debugging
+
+//#define GPSDEBUG
+
+#ifdef GPSDEBUG
+        #define NMEA_DEBUG_PKT	///< define to enable debug of all NMEA messages
+        #define NMEA_DEBUG_GGA	///< define to enable debug of GGA messages
+        #define NMEA_DEBUG_VTG	///< define to enable debug of VTG messages
+        #define NMEA_DEBUG_RMC	///< define to enable debug of RMC messages
+        #define NMEA_DEBUG_GSA	///< define to enable debug of GSA messages
+#endif
+
+// Private functions
+
+// Global variables
+
+// Private constants
+typedef struct struct_GpsData
+{
+        float Latitude;
+        float Longitude;
+        float Altitude;
+        float Groundspeed;
+        int SV;
+        uint8_t channel;
+        uint8_t value_h;
+        uint8_t value_l;
+        uint8_t sum;
+}GpsData_t;
+
+// Private types
+
+// Private variables
+cBuffer gpsRxBuffer;
+static char gpsRxData[512];
+char NmeaPacket[NMEA_BUFFERSIZE];
+static uint32_t numUpdates;
+static uint32_t numErrors;
+static uint32_t timeOfLastUpdateMs;
+static int32_t gpsRxOverflow=0;
+GpsData_t GpsData;
+
+
 
 class MyThread : public QThread
 {
 public:
     QextSerialPort *port;
     void setPort(QextSerialPort* port);
+    void processInputStream();
     void run();
 };
 
@@ -51,6 +110,8 @@ public:
  */
 GpsDisplayWidget::GpsDisplayWidget(QWidget *parent) : QWidget(parent)
 {
+    bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 512);
+
     setMinimumSize(128,128);
     setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding);
  
@@ -97,6 +158,615 @@ void GpsDisplayWidget::connectButtonClicked() {
 }
 
 
+
+/**
+ * Prosesses NMEA sentence checksum
+ * \param[in] Buffer for parsed nmea sentence
+ * \return 0 checksum not valid
+ * \return 1 checksum valid
+ */
+char GpsDisplayWidget::nmeaChecksum(char* gps_buffer)
+{
+        char checksum=0;
+        char checksum_received=0;
+
+        for(int x=0; x<NMEA_BUFFERSIZE; x++)
+        {
+                if(gps_buffer[x]=='*')
+                {
+                        //Parsing received checksum...
+                        checksum_received = strtol(&gps_buffer[x+1],NULL,16);
+                        break;
+                }
+                else
+                {
+                        //XOR the received data...
+                        checksum^=gps_buffer[x];
+                }
+        }
+        //PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum);
+        if(checksum == checksum_received)
+        {
+                ++numUpdates;
+                return 1;
+        }
+        else
+        {
+                ++numErrors;
+                return 0;
+        }
+}
+
+/**
+ * Prosesses NMEA sentences
+ * \param[in] cBuffer for prosessed nmea sentences
+ * \return Message code for found packet
+ * \return 0xFF NO packet found
+ */
+uint8_t GpsDisplayWidget::nmeaProcess(cBuffer* rxBuffer)
+{
+        uint8_t foundpacket = NMEA_NODATA;
+        uint8_t startFlag = FALSE;
+        //u08 data;
+        uint16_t 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
+                                if (j<(NMEA_BUFFERSIZE-1)) {
+                                        NmeaPacket[j] = 0;
+                                } else {
+                                        NmeaPacket[NMEA_BUFFERSIZE-1] = 0;
+                                }
+                                // dump <CR><LF> from rxBuffer
+                                bufferGetFromFront(rxBuffer);
+                                bufferGetFromFront(rxBuffer);
+                                //DEBUG
+                                #ifdef NMEA_DEBUG_PKT
+                                //PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",NmeaPacket);
+                                #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(!strncmp(NmeaPacket, "GPGSA", 5))
+                {
+                        // process packet of this type
+                        nmeaProcessGPGSA(NmeaPacket);
+                        // report packet type
+                        foundpacket = NMEA_GPGSA;
+                }
+                else if(!strncmp(NmeaPacket, "GPRMC", 5))
+                {
+                        // process packet of this type
+                        nmeaProcessGPRMC(NmeaPacket);
+                        // report packet type
+                        foundpacket = NMEA_GPRMC;
+                }
+        }
+        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;
+}
+
+/**
+ * Prosesses NMEA GPGGA sentences
+ * \param[in] Buffer for parsed nmea GPGGA sentence
+ */
+void GpsDisplayWidget::nmeaProcessGPGGA(char* packet)
+{
+        // start parsing just after "GPGGA,"
+        // attempt to reject empty packets right away
+        if(packet[6]==',' && packet[7]==',')
+                return;
+
+        if(!nmeaChecksum(packet))
+        {
+                // checksum not valid
+                return;
+        }
+
+        QString* nmeaString = new QString( packet );
+        QStringList tokenslist = nmeaString->split(",");
+        GpsData.Latitude = tokenslist.at(2).toFloat();
+        GpsData.Longitude = tokenslist.at(4).toFloat();
+        GpsData.Altitude = tokenslist.at(9).toFloat();
+        GpsData.SV = tokenslist.at(7).toInt();
+
+/*
+        char *tokens;
+    char *delimiter = ",";
+    char *pEnd;
+    char token_length=0;
+
+    long deg,min,desim;
+
+        #ifdef NMEA_DEBUG_GGA
+                PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
+        #endif
+
+        // start parsing just after "GPGGA,"
+        // attempt to reject empty packets right away
+        if(packet[6]==',' && packet[7]==',')
+                return;
+
+        if(!nmeaChecksum(packet))
+        {
+                // checksum not valid
+                return;
+        }
+
+        // tokenizer for nmea sentence
+        //GPGGA header
+        tokens = strsep(&packet, delimiter);
+
+        // get UTC time [hhmmss.sss]
+        tokens = strsep(&packet, delimiter);
+        //strcpy(GpsInfo.TimeOfFix,tokens);
+
+        // next field: latitude
+    // get latitude [ddmm.mmmmm]
+        tokens = strsep(&packet, delimiter);
+        token_length=strlen(tokens);
+        deg=strtol (tokens,&pEnd,10);
+        min=deg%100;
+        deg=deg/100;
+        desim=strtol (pEnd+1,NULL,10);
+        if(token_length==10)// 5 desimal output
+        {
+                GpsData.Latitude=deg+(min+desim/100000.0)/60.0; // to desimal degrees
+        }
+        else if(token_length==9) // 4 desimal output
+        {
+                GpsData.Latitude=deg+(min+desim/10000.0)/60.0; // to desimal degrees
+        }
+        else if(token_length==11) // 6 desimal output OPGPS
+        {
+                GpsData.Latitude=deg+(min+desim/1000000.0)/60.0; // to desimal degrees
+        }
+
+    // next field: N/S indicator
+        // correct latitute for N/S
+        tokens = strsep(&packet, delimiter);
+        if(tokens[0] == 'S') GpsData.Latitude = -GpsData.Latitude;
+
+        // next field: longitude
+        // get longitude [dddmm.mmmmm]
+        tokens = strsep(&packet, delimiter);
+        token_length=strlen(tokens);
+        deg=strtol (tokens,&pEnd,10);
+        min=deg%100;
+        deg=deg/100;
+        desim=strtol (pEnd+1,NULL,10);
+        if(token_length==11)// 5 desimal output
+        {
+                GpsData.Longitude=deg+(min+desim/100000.0)/60.0; // to desimal degrees
+        }
+        else if(token_length==10) // 4 desimal output
+        {
+                GpsData.Longitude=deg+(min+desim/10000.0)/60.0; // to desimal degrees
+        }
+        else if(token_length==12) // 6 desimal output OPGPS
+        {
+                GpsData.Longitude=deg+(min+desim/1000000.0)/60.0; // to desimal degrees
+        }
+    // next field: E/W indicator
+        // correct latitute for E/W
+        tokens = strsep(&packet, delimiter);
+        if(tokens[0] == 'W') GpsData.Longitude = -GpsData.Longitude;
+
+    // next field: position fix status
+        // position fix status
+        // 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
+        // check for good position fix
+        tokens = strsep(&packet, delimiter);
+    //if((tokens[0] != '0') || (tokens[0] != 0))
+    //	GpsData.Updates++;
+
+    // next field: satellites used
+    // get number of satellites used in GPS solution
+    tokens = strsep(&packet, delimiter);
+        GpsData.Satellites=atoi(tokens);
+
+        // next field: HDOP (horizontal dilution of precision)
+        tokens = strsep(&packet, delimiter);
+
+        // next field: altitude
+        // get altitude (in meters mm.m)
+        tokens = strsep(&packet, delimiter);
+        //reuse variables for alt
+        deg=strtol (tokens,&pEnd,10); // always 0.1m resolution?
+        desim=strtol (pEnd+1,NULL,10);
+        if(1) // OPGPS 3 desimal
+                GpsData.Altitude=deg+desim/1000.0;
+        else
+                GpsData.Altitude=deg+desim/10.0;
+
+        // next field: altitude units, always 'M'
+        // next field: geoid seperation
+        // next field: seperation units
+        // next field: DGPS age
+        // next field: DGPS station ID
+        // next field: checksum
+        */
+}
+
+/**
+ * Prosesses NMEA GPRMC sentences
+ * \param[in] Buffer for parsed nmea GPRMC sentence
+ */
+void GpsDisplayWidget::nmeaProcessGPRMC(char* packet)
+{
+        // start parsing just after "GPRMC,"
+        // attempt to reject empty packets right away
+        if(packet[6]==',' && packet[7]==',')
+                return;
+
+        if(!nmeaChecksum(packet))
+        {
+                // checksum not valid
+                return;
+        }
+
+        QString* nmeaString = new QString( packet );
+        QStringList tokenslist = nmeaString->split(",");
+        GpsData.Groundspeed = tokenslist.at(7).toFloat();//(deg+(desim/100.0))*0.51444; //OPGPS style to m/s
+        GpsData.Groundspeed = GpsData.Groundspeed*0.51444*3.6;
+
+/*
+        char *tokens;
+    char *delimiter = ",";
+    char *pEnd;
+    char token_length=0;
+
+    long deg,min,desim;
+
+        #ifdef NMEA_DEBUG_RMC
+                PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
+        #endif
+
+        // start parsing just after "GPRMC,"
+        // attempt to reject empty packets right away
+        if(packet[6]==',' && packet[7]==',')
+                return;
+
+        if(!nmeaChecksum(packet))
+        {
+                // checksum not valid
+                return;
+        }
+
+        // tokenizer for nmea sentence
+        //GPRMC header
+        tokens = strsep(&packet, delimiter);
+
+        // get UTC time [hhmmss.sss]
+        tokens = strsep(&packet, delimiter);
+        //strcpy(GpsInfo.TimeOfFix,tokens);
+        // next field: Navigation receiver warning A = OK, V = warning
+        tokens = strsep(&packet, delimiter);
+
+        // next field: latitude
+    // get latitude [ddmm.mmmmm]
+        tokens = strsep(&packet, delimiter);
+        token_length=strlen(tokens);
+        deg=strtol (tokens,&pEnd,10);
+        min=deg%100;
+        deg=deg/100;
+        desim=strtol (pEnd+1,NULL,10);
+        /*if(token_length==10)// 5 desimal output
+        {
+                GpsData.Latitude=deg+(min+desim/100000.0)/60.0; // to desimal degrees
+        }
+        else if(token_length==9) // 4 desimal output
+        {
+                GpsData.Latitude=deg+(min+desim/10000.0)/60.0; // to desimal degrees
+        }
+        else if(token_length==11) // 6 desimal output OPGPS
+        {
+                GpsData.Latitude=deg+(min+desim/1000000.0)/60.0; // to desimal degrees
+        }*
+
+    // next field: N/S indicator
+        // correct latitute for N/S
+        tokens = strsep(&packet, delimiter);
+        //if(tokens[0] == 'S') GpsData.Latitude = -GpsData.Latitude;
+
+        // next field: longitude
+        // get longitude [dddmm.mmmmm]
+        tokens = strsep(&packet, delimiter);
+        token_length=strlen(tokens);
+        deg=strtol (tokens,&pEnd,10);
+        min=deg%100;
+        deg=deg/100;
+        desim=strtol (pEnd+1,NULL,10);
+        /*if(token_length==11)// 5 desimal output
+        {
+                GpsData.Longitude=deg+(min+desim/100000.0)/60.0; // to desimal degrees
+        }
+        else if(token_length==10) // 4 desimal output
+        {
+                GpsData.Longitude=deg+(min+desim/10000.0)/60.0; // to desimal degrees
+        }
+        else if(token_length==12) // 6 desimal output OPGPS
+        {
+                GpsData.Longitude=deg+(min+desim/1000000.0)/60.0; // to desimal degrees
+        }*
+    // next field: E/W indicator
+        // correct latitute for E/W
+        tokens = strsep(&packet, delimiter);
+        //if(tokens[0] == 'W') GpsData.Longitude = -GpsData.Longitude;
+
+        // next field: speed (knots)
+        // get speed in knots
+        tokens = strsep(&packet, delimiter);
+        deg=strtol (tokens,&pEnd,10);
+        desim=strtol (pEnd+1,NULL,10);
+        GpsData.Groundspeed = (deg+(desim/100.0))*0.51444; //OPGPS style to m/s
+
+        // next field: True course
+        // get True course
+        tokens = strsep(&packet, delimiter);
+        deg=strtol (tokens,&pEnd,10);
+        desim=strtol (pEnd+1,NULL,10);
+        GpsData.Heading = deg+(desim/100.0); //OPGPS style
+
+        // next field: Date of fix
+        // get Date of fix
+        tokens = strsep(&packet, delimiter);
+
+        // next field: Magnetic variation
+        // next field: E or W
+        // next field: checksum
+        */
+}
+
+
+/**
+ * Prosesses NMEA GPVTG sentences
+ * \param[in] Buffer for parsed nmea GPVTG sentence
+ */
+void GpsDisplayWidget::nmeaProcessGPVTG(char* packet)
+{
+        // start parsing just after "GPVTG,"
+        // attempt to reject empty packets right away
+        if(packet[6]==',' && packet[7]==',')
+                return;
+
+        if(!nmeaChecksum(packet))
+        {
+                // checksum not valid
+                return;
+        }
+/*
+        char *tokens;
+    char *delimiter = ",";
+
+        #ifdef NMEA_DEBUG_VTG
+                PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
+        #endif
+
+        // start parsing just after "GPVTG,"
+        // attempt to reject empty packets right away
+        if(packet[6]==',' && packet[7]==',')
+                return;
+
+        if(!nmeaChecksum(packet))
+        {
+                // checksum not valid
+                return;
+        }
+        // tokenizer for nmea sentence
+
+        //GPVTG header
+        tokens = strsep(&packet, delimiter);
+
+        // get course (true north ref) in degrees [ddd.dd]
+        tokens = strsep(&packet, delimiter);
+    // next field: 'T'
+        tokens = strsep(&packet, delimiter);
+
+    // next field: course (magnetic north)
+        // get course (magnetic north ref) in degrees [ddd.dd]
+        tokens = strsep(&packet, delimiter);
+        // next field: 'M'
+        tokens = strsep(&packet, delimiter);
+
+        // next field: speed (knots)
+        // get speed in knots
+        tokens = strsep(&packet, delimiter);
+        // next field: 'N'
+        tokens = strsep(&packet, delimiter);
+
+        // next field: speed (km/h)
+        // get speed in km/h
+        tokens = strsep(&packet, delimiter);
+        // next field: 'K'
+        // next field: checksum
+*/
+}
+
+/**
+ * Prosesses NMEA GPGSA sentences
+ * \param[in] Buffer for parsed nmea GPGSA sentence
+ */
+void GpsDisplayWidget::nmeaProcessGPGSA(char* packet)
+{
+        // start parsing just after "GPGSA,"
+        // attempt to reject empty packets right away
+        if(packet[6]==',' && packet[7]==',')
+                return;
+
+        if(!nmeaChecksum(packet))
+        {
+                // checksum not valid
+                return;
+        }
+/*
+        char *tokens;
+    char *delimiter = ",";
+    char *pEnd;
+    long value,desim;
+    int mode;
+
+        #ifdef NMEA_DEBUG_GSA
+                PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
+        #endif
+
+        // start parsing just after "GPGSA,"
+        // attempt to reject empty packets right away
+        if(packet[6]==',' && packet[7]==',')
+                return;
+
+        if(!nmeaChecksum(packet))
+        {
+                // checksum not valid
+                return;
+        }
+        // tokenizer for nmea sentence
+
+        //GPGSA header
+        tokens = strsep(&packet, delimiter);
+
+    // next field: Mode
+        // Mode: M=Manual, forced to operate in 2D or 3D, A=Automatic, 3D/2D
+        tokens = strsep(&packet, delimiter);
+    // next field: Mode
+    // Mode: 1=Fix not available, 2=2D, 3=3D
+        tokens = strsep(&packet, delimiter);
+        mode = atoi(tokens);
+        if (mode == 1)
+        {
+                GpsData.Status = POSITIONACTUAL_STATUS_NOFIX;
+        }
+        else if (mode == 2)
+        {
+                GpsData.Status = POSITIONACTUAL_STATUS_FIX2D;
+        }
+        else if (mode == 3)
+        {
+                GpsData.Status = POSITIONACTUAL_STATUS_FIX3D;
+        }
+
+    // next field: 3-14 IDs of SVs used in position fix (null for unused fields)
+        tokens = strsep(&packet, delimiter);
+        tokens = strsep(&packet, delimiter);
+        tokens = strsep(&packet, delimiter);
+        tokens = strsep(&packet, delimiter);
+        tokens = strsep(&packet, delimiter);
+        tokens = strsep(&packet, delimiter);
+        tokens = strsep(&packet, delimiter);
+        tokens = strsep(&packet, delimiter);
+        tokens = strsep(&packet, delimiter);
+        tokens = strsep(&packet, delimiter);
+        tokens = strsep(&packet, delimiter);
+        tokens = strsep(&packet, delimiter);
+
+        // next field: PDOP
+        tokens = strsep(&packet, delimiter);
+        value=strtol (tokens,&pEnd,10);
+        desim=strtol (pEnd+1,NULL,10);
+        GpsData.PDOP=value+desim/100.0;
+        // next field: HDOP
+        tokens = strsep(&packet, delimiter);
+        value=strtol (tokens,&pEnd,10);
+        desim=strtol (pEnd+1,NULL,10);
+        GpsData.HDOP=value+desim/100.0;
+        // next field: VDOP
+        tokens = strsep(&packet, delimiter);
+        value=strtol (tokens,&pEnd,10);
+        desim=strtol (pEnd+1,NULL,10);
+        GpsData.VDOP=value+desim/100.0;
+        // next field: checksum
+*/
+}
+
+
+
+/**
+ * Called each time there are data in the input buffer
+ */
+void MyThread::processInputStream()
+{
+    GpsDisplayWidget *widget;
+    //quint8 tmp;
+        char c=0;
+        port->read(&c,1);
+        if( !bufferAddToEnd(&gpsRxBuffer, c) )
+        {
+                // no space in buffer
+                // count overflow
+                gpsRxOverflow++;
+                return;
+        }
+        widget->nmeaProcess(&gpsRxBuffer);
+}
+
 void MyThread::setPort(QextSerialPort* port)
 {
 
@@ -115,10 +785,14 @@ void MyThread::run()
     char buf[1024];
     while(true) {
         qDebug() <<  "Reading.";
-        qint64 bytesRead = port->readLine(buf, sizeof(buf));
+        /*qint64 bytesRead = port->readLine(buf, sizeof(buf));
         qDebug() << "bytesRead: " << bytesRead;
         if (bytesRead != -1) {
             qDebug() <<  "Result: '" << buf << "'";
+        }*/
+        while(port->bytesAvailable()>0)
+        {
+                processInputStream();
         }
         sleep(1);
     }

ground/src/plugins/gpsdisplay/gpsdisplaywidget.h

@@ -36,6 +36,7 @@
 
 #include <QFile>
 #include <QTimer>
+#include "buffer.h"
 
 class Ui_GpsDisplayWidget;
 
@@ -49,7 +50,13 @@ public:
 
 //   void setMode(QString mode);  // Either UAVTalk or serial port
    void setPort(QextSerialPort* port);
-
+   char* nmeaGetPacketBuffer(void);
+   char nmeaChecksum(char* gps_buffer);
+   uint8_t nmeaProcess(cBuffer* rxBuffer);
+   void nmeaProcessGPGGA(char* packet);
+   void nmeaProcessGPRMC(char* packet);
+   void nmeaProcessGPVTG(char* packet);
+   void nmeaProcessGPGSA(char* packet);
 
 private slots:
    void connectButtonClicked();