diff --git a/ground/src/plugins/gpsdisplay/buffer.cpp b/ground/src/plugins/gpsdisplay/buffer.cpp new file mode 100644 index 000000000..9fe0e4197 --- /dev/null +++ b/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; +} + diff --git a/ground/src/plugins/gpsdisplay/buffer.h b/ground/src/plugins/gpsdisplay/buffer.h new file mode 100644 index 000000000..3a2ca9808 --- /dev/null +++ b/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 +//@} diff --git a/ground/src/plugins/gpsdisplay/gpsdisplay.pro b/ground/src/plugins/gpsdisplay/gpsdisplay.pro index f532fe1f7..8de754699 100644 --- a/ground/src/plugins/gpsdisplay/gpsdisplay.pro +++ b/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 diff --git a/ground/src/plugins/gpsdisplay/gpsdisplaywidget.cpp b/ground/src/plugins/gpsdisplay/gpsdisplaywidget.cpp index ec3ed811c..fedd93e05 100644 --- a/ground/src/plugins/gpsdisplay/gpsdisplaywidget.cpp +++ b/ground/src/plugins/gpsdisplay/gpsdisplaywidget.cpp @@ -35,12 +35,71 @@ #include #include +// 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; xdatalength) + { + // 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 + 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 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); } diff --git a/ground/src/plugins/gpsdisplay/gpsdisplaywidget.h b/ground/src/plugins/gpsdisplay/gpsdisplaywidget.h index 1287644bf..a08914781 100644 --- a/ground/src/plugins/gpsdisplay/gpsdisplaywidget.h +++ b/ground/src/plugins/gpsdisplay/gpsdisplaywidget.h @@ -36,6 +36,7 @@ #include #include +#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();