📄 nmeaparser.cpp
字号:
//---------------------------------------------------------------------------
#include <sstream>
#pragma hdrstop
#include "NMEAParser.h"
//---------------------------------------------------------------------------
#pragma package(smart_init)
//---------------------------------------------------------------------------
TNmeaSentence::TNmeaSentence()
{
MachineState=STATE_SOM;
}
//---------------------------------------------------------------------------
TNmeaSentence::~TNmeaSentence()
{
}
//---------------------------------------------------------------------------
// ParseBuffer: Parse the supplied buffer for NMEA sentence information. Since
// the parser is a state machine, partial NMEA sentence data may
// be supplied where the next time this method is called, the
// rest of the partial NMEA data will complete the sentence.
//
// A typical NMEA sentence is constructed as:
//
// $CMD,DDDD,DDDD,....DD*CS<CR><LF>
//
// Where:
// '$' HEX 24 Start of sentence
// 'CMD' Address/NMEA command
// ',DDDD' Zero or more data fields
// '*CS' Checksum field
// <CR><LF> Hex 0d 0A End of sentence
//
// When a valid sentence is received, this method will store parsed
// NMEA command and data in the class internally. To retrive these
// information for application use, such as monitoring GPS output,
// logging data etc, call RetriveSentences() to extract them.
//
// Also, the function will return number of sentences parsed, the
// caller can get these sentences by access the member Sentence.
//
bool TNmeaSentence::ParseBuffer(char *Buffer, int BufferLength)
{
const int MaxCommandLength=5;
const int MaxDataLength=256;
// Delete last time sentences.
Sentences.clear();
// Parsing NMEA commands by using a state machine.
for(int i=0; i<BufferLength; i++){
switch(MachineState){
// Search for start of sentence character, '$'.
case STATE_SOM:
if(Buffer[i]=='$'){ // Begin of sentence.
MachineState=STATE_CMD; // Tune to get command.
SenBuff.Command=""; // Empty command buffer.
SenBuff.Data=""; // Empty data buffer.
SenBuff.Checksum=0; // Reset checksum.
}
break;
// Retrieve command (also called 'NMEA Address').
case STATE_CMD:
if(Buffer[i]!=',' && Buffer[i]!='*'){
// Command character found.
if(SenBuff.Command.length()<MaxCommandLength){
SenBuff.Command.push_back(Buffer[i]);
SenBuff.Checksum^=Buffer[i]; // Calculate check sum.
}else{
// Command is too long, restart the state machine.
MachineState=STATE_SOM;
}
}else{
// Next state will get data.
if(SenBuff.Command.length()==MaxCommandLength){
SenBuff.Checksum^=Buffer[i];
MachineState=STATE_DATA; // Goto get data state.
}else{
// Command is too short, restart the state machine.
MachineState=STATE_SOM;
}
}
break;
// Store data and check for end of sentence or checksum flag.
case STATE_DATA:
if(Buffer[i]=='*'){ // Checksum flag?
// Next state is CHECKSUM1.
MachineState=STATE_CHECKSUM1;
}else{
if(Buffer[i]!='\r' && Buffer[i]!='\n'){
// Store data.
if(SenBuff.Data.length()<MaxDataLength){
SenBuff.Data.push_back(Buffer[i]);
SenBuff.Checksum^=Buffer[i];
}else{
// Data is too long, restart the state machine.
MachineState=STATE_SOM;
}
}else{
// A sentence without checksum has been found.
// Save parsed information.
Sentences.push_back(SenBuff);
MachineState=STATE_SOM;
}
}
break;
// Calculate first check sum character, base 16 formated.
case STATE_CHECKSUM1:
if((Buffer[i]-'0')<=9){
ChecksumReceived=(Buffer[i]-'0')<<4;
}else{
ChecksumReceived=(Buffer[i]-'A'+10)<<4;
}
MachineState=STATE_CHECKSUM2;
break;
// Calculate last check sum character, base 16 formated.
case STATE_CHECKSUM2:
if((Buffer[i]-'0')<=9){
ChecksumReceived|=(Buffer[i]-'0');
}else{
ChecksumReceived|=(Buffer[i]-'A'+10);
}
// Check for final checksum.
if(ChecksumReceived==SenBuff.Checksum){
// Checksum validated, save parsed information.
Sentences.push_back(SenBuff);
}
// Whole sentence parsed, restart the state machine any way.
MachineState=STATE_SOM;
break;
// With another undefined states.
default:
MachineState=STATE_SOM;
break;
}
}
// Return at least one sentence has been retained.
return Sentences.size()>0;
}
//---------------------------------------------------------------------------
int TNmeaSentence::RetrieveSentences(vector<string> &SentenceOut)
{
const char Base16[]={
'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'
};
if(Sentences.size()==0){
return 0;
}
// Organize new NMEA sentences with new checksum append.
vector<SentenceData>::iterator SenBuf=Sentences.begin();
while(SenBuf!=Sentences.end()){
SentenceOut.push_back("$"+(*SenBuf).Command+","+(*SenBuf).Data+"*"
+Base16[(*SenBuf).Checksum>>4]+Base16[(*SenBuf).Checksum&15]);
SenBuf++;
}
return (int)Sentences.size();
}
//---------------------------------------------------------------------------
// Get the specified field in a NMEA data string.
bool TNmeaSentence::GetField(string &Data, int FieldNumber, string &Out)
{
if(Data.empty()){
return false;
}
// Go to the beginning of the selected field.
string::size_type i=0;
int Field=0;
while(Field!=FieldNumber && i<Data.length()){
if(Data[i++]==','){
Field++;
}
if(i==Data.length()){
return false;
}
}
if(Data[i]==','){
return false;
}
// Copy field data.
Out.clear();
while(Data[i]!= ',' && i<Data.length()){
Out.push_back(Data[i++]);
}
return true;
}
//---------------------------------------------------------------------------
// Get the specified field in a NMEA string.
bool TNmeaSentence::GetField(string &Data, int FieldNumber, int &Out)
{
string tmp;
if(!GetField(Data, FieldNumber, tmp)){
return false;
}
istringstream is(tmp);
is>>Out;
return true;
}
//---------------------------------------------------------------------------
// Get the specified field in a NMEA string.
bool TNmeaSentence::GetField(string &Data, int FieldNumber, double &Out)
{
string tmp;
if(!GetField(Data, FieldNumber, tmp)){
return false;
}
istringstream is(tmp);
is>>Out;
return true;
}
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
TNmeaData::TNmeaData()
{
}
//---------------------------------------------------------------------------
TNmeaData::~TNmeaData()
{
}
//---------------------------------------------------------------------------
bool TNmeaData::ParseBuffer(char *Buffer, int BufferLength)
{
if(!TNmeaSentence::ParseBuffer(Buffer, BufferLength)){
return false;
}
vector<SentenceData>::iterator SenBuf=Sentences.begin();
while(SenBuf!=Sentences.end()){
if((*SenBuf).Command=="GPGGA"){
return Gpgga.ParseData((*SenBuf).Data);
}
if((*SenBuf).Command=="GPGSA"){
return Gpgsa.ParseData((*SenBuf).Data);
}
SenBuf++;
}
return true;
}
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
TNmeaData::GpggaData::GpggaData()
{
Hour=0;
Minute=0;
Second=0;
Latitude=0;
Longitude=0;
GpsQuality=0;
SatellitesInUse=0;
HDop=0;
AntennaHeight=0;
AntennaUnit="M";
GeoidalHeight=0;
GeoidalUnit="M";
DGpsDataAge=0;
DGpsStationId=0;
Count=0;
OldVSpeedSeconds=0;
OldVSpeedAlt=0;
VerticalSpeed=0;
}
//---------------------------------------------------------------------------
bool TNmeaData::GpggaData::ParseData(string &Data)
{
string Field;
stringstream is;
double tmpd;
// Get time.
if(GetField(Data, 0, Field) && Field.length()<=10){
// Hour
is<<Field.substr(0, 2)<<" "<<Field.substr(2, 2)<<" "<<Field.substr(4, Field.length()-4);
is>>Hour>>Minute>>Second;
is.clear();
}
// Get latitude.
if(GetField(Data, 1, Field)){
is<<Field.substr(2, Field.length()-2)<<" "<<Field.substr(0, 2);
is>>tmpd>>Latitude;
is.clear();
Latitude+=tmpd/60.0;
GetField(Data, 2, Field);
if(Field[0]=='S'){
Latitude*=-1;
}
}
// Get longitude.
if(GetField(Data, 3, Field)){
is<<Field.substr(3, Field.length()-3)<<" "<<Field.substr(0, 3);
is>>tmpd>>Longitude;
is.clear();
Longitude+=tmpd/60.0;
GetField(Data, 4, Field);
if(Field[0]=='W'){
Longitude*=-1;
}
}
// Get GPS quality.
GetField(Data, 5, GpsQuality);
// Get satellites in use.
GetField(Data, 6, SatellitesInUse);
// Get HDop.
GetField(Data, 7, HDop);
// Get AntennaHeight.
GetField(Data, 8, AntennaHeight);
// Get AntennaUnit.
GetField(Data, 9, AntennaUnit);
// Get GeoidalHeight.
GetField(Data, 10, GeoidalHeight);
// Get GeoidalUnit.
GetField(Data, 11, GeoidalUnit);
// Get DGpsDataAge.
GetField(Data, 12, DGpsDataAge);
// Get DGpsStationId.
GetField(Data, 13, DGpsStationId);
// Durive vertical speed (bonus), :):)
double Seconds=Minute*60.0+Second;
if(Seconds>OldVSpeedSeconds){
double Diff=OldVSpeedSeconds-Seconds;
double Val=Diff/60.0;
if(Val!=0.0){
VerticalSpeed=(OldVSpeedAlt-(AntennaHeight+GeoidalHeight))/Val;
}
}
OldVSpeedAlt=AntennaHeight+GeoidalHeight;
OldVSpeedSeconds=Seconds;
Count++;
return true;
}
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
TNmeaData::GpgsaData::GpgsaData()
{
Mode="M";
FixMode=1;
PDop=0;
HDop=0;
VDop=0;
Count=0;
}
//---------------------------------------------------------------------------
bool TNmeaData::GpgsaData::ParseData(string &Data)
{
int tmpi;
// Get Fix Mode.
GetField(Data, 0, Mode);
// Get Fix Type.
GetField(Data, 1, FixMode);
// Get satellites list that in use.
PrnNumber.clear();
for(int i=0; i<12; i++){
if(GetField(Data, 2+i, tmpi)){
PrnNumber.push_back(tmpi);
}
}
// Get PDOP.
GetField(Data, 14, PDop);
// Get HDOP.
GetField(Data, 15, HDop);
// Get VDOP.
GetField(Data, 16, VDop);
Count++;
return true;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -