mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
c53aa219d5
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@691 ebee16cc-31ac-478f-84a7-5cbb03baadba
668 lines
18 KiB
C
668 lines
18 KiB
C
/**
|
|
******************************************************************************
|
|
*
|
|
* @file GPS.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @brief GPS module, handles GPS and NMEA stream
|
|
* @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
|
|
*/
|
|
|
|
#include "openpilot.h"
|
|
#include "buffer.h"
|
|
#include "GPS.h"
|
|
#include "gpsobject.h"
|
|
|
|
// 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
|
|
|
|
// 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
|
|
static void gpsTask(void* parameters);
|
|
|
|
// functions
|
|
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);
|
|
|
|
// Global variables
|
|
|
|
// Private constants
|
|
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
|
|
|
|
// Private types
|
|
|
|
// Private variables
|
|
static COMPortTypeDef gpsPort;
|
|
static xTaskHandle gpsTaskHandle;
|
|
cBuffer gpsRxBuffer;
|
|
static char gpsRxData[512];
|
|
char NmeaPacket[NMEA_BUFFERSIZE];
|
|
|
|
/**
|
|
* Initialise the gps module
|
|
* \return -1 if initialisation failed
|
|
* \return 0 on success
|
|
*/
|
|
int32_t GpsInitialize(void)
|
|
{
|
|
signed portBASE_TYPE xReturn;
|
|
// TODO: Get gps settings object
|
|
gpsPort = COM_USART2;
|
|
|
|
// Init input buffer size 512
|
|
bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 512);
|
|
|
|
// Start gps task
|
|
xReturn = xTaskCreate(gpsTask, (signed char*)"GPS", STACK_SIZE, NULL, TASK_PRIORITY, &gpsTaskHandle);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
/**
|
|
* gps task. Processes input buffer. It does not return.
|
|
*/
|
|
static void gpsTask(void* parameters)
|
|
{
|
|
int32_t gpsRxOverflow=0;
|
|
char c;
|
|
portTickType xDelay = 100 / portTICK_RATE_MS;
|
|
|
|
// Loop forever
|
|
while(1)
|
|
{
|
|
/* This blocks the task until there is something on the buffer */
|
|
while(PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
|
|
{
|
|
c=PIOS_COM_ReceiveBuffer(gpsPort);
|
|
if( !bufferAddToEnd(&gpsRxBuffer, c) )
|
|
{
|
|
// no space in buffer
|
|
// count overflow
|
|
gpsRxOverflow++;
|
|
break;
|
|
}
|
|
nmeaProcess(&gpsRxBuffer);
|
|
}
|
|
vTaskDelay(xDelay);
|
|
}
|
|
}
|
|
|
|
char* nmeaGetPacketBuffer(void)
|
|
{
|
|
return NmeaPacket;
|
|
}
|
|
|
|
/**
|
|
* Prosesses NMEA sentence checksum
|
|
* \param[in] Buffer for parsed nmea sentence
|
|
* \return 0 checksum not valid
|
|
* \return 1 checksum valid
|
|
*/
|
|
char nmeaChecksum(char* gps_buffer)
|
|
{
|
|
char checksum=0;
|
|
char checksum_received=0;
|
|
GpsObjectData GpsData;
|
|
GpsObjectGet(&GpsData);
|
|
|
|
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)
|
|
{
|
|
GpsData.Updates++;
|
|
GpsObjectSet(&GpsData);
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
GpsData.Failures++;
|
|
GpsObjectSet(&GpsData);
|
|
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 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
|
|
NmeaPacket[j] = 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 nmeaProcessGPGGA(char* packet)
|
|
{
|
|
GpsObjectData GpsData;
|
|
|
|
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;
|
|
}
|
|
|
|
GpsObjectGet(&GpsData);
|
|
// 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
|
|
GpsObjectSet(&GpsData);
|
|
}
|
|
|
|
/**
|
|
* Prosesses NMEA GPRMC sentences
|
|
* \param[in] Buffer for parsed nmea GPRMC sentence
|
|
*/
|
|
void nmeaProcessGPRMC(char* packet)
|
|
{
|
|
GpsObjectData GpsData;
|
|
|
|
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;
|
|
}
|
|
|
|
GpsObjectGet(&GpsData);
|
|
// 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
|
|
GpsObjectSet(&GpsData);
|
|
}
|
|
|
|
|
|
/**
|
|
* Prosesses NMEA GPVTG sentences
|
|
* \param[in] Buffer for parsed nmea GPVTG sentence
|
|
*/
|
|
void nmeaProcessGPVTG(char* packet)
|
|
{
|
|
GpsObjectData GpsData;
|
|
|
|
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;
|
|
}
|
|
GpsObjectGet(&GpsData);
|
|
// 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
|
|
GpsObjectSet(&GpsData);
|
|
|
|
}
|
|
|
|
/**
|
|
* Prosesses NMEA GPGSA sentences
|
|
* \param[in] Buffer for parsed nmea GPGSA sentence
|
|
*/
|
|
void nmeaProcessGPGSA(char* packet)
|
|
{
|
|
GpsObjectData GpsData;
|
|
|
|
char *tokens;
|
|
char *delimiter = ",";
|
|
char *pEnd;
|
|
long value,desim;
|
|
|
|
#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;
|
|
}
|
|
GpsObjectGet(&GpsData);
|
|
// 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);
|
|
|
|
// 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
|
|
GpsObjectSet(&GpsData);
|
|
|
|
}
|
|
|
|
|