NEMA协议的解析,c++

因为是读串口,每次读进来的数据都不一定是完整的,很可能是一小段一小段的来,需要在内存中拼凑完整并验证完成后解析。

 

NEMA_decoder  输入为字符串,输出为状态集,即本次输入能得到几个消息。

头文件

//GPS协议中常用到的字符

 

#define DLE (0x0D)

#define ETX (0x0A)

#define HED         (0x24)

#define CMA         (0x2C)//逗号

#define STAR        (0x2A) 

//解析状态

enum decoder_e

{

NONE_e  = 0,

GPRMC_e = 1,

GPGSV_e = 1 << 1,

GPGGA_e = 1 << 2,

GPGSA_e = 1 << 3,

};

 

//GPRMC协议

struct GPRMC_m

{

int UTC_Time;

int UTC_Date;

char status;

char latitude;

char longtitude;

char reverse1;

float latitude_value;

float longtitude_value;

float speed;

float azimuth_angle;

};

//GPGGA

struct GPGGA_m

{

int UTC_Time;

char lat;

char lon;

char  status;

char reverse1;

float latValue;

float lonValue;

int  sate_num;

};

 

 

struct GPGSA_m{

char mode1;

char mode2;

char channel[12];

char rev[2];

float pdop;

float hdop;

float vdop;

};

struct GPGSV_m

{

int sate_id;

int degree1;

int degree2;

int ndb;

};

struct GPGSV_h_m

{

int gsvNum;

int curId;

int sateNum;

int reverse;

};

enum VALUE_e

{

FLOAT_e,

INT_e,

STRING_e,

CHAR_e,

};

struct input_t

{

VALUE_e type;

void* pointer;

};

class NEMA_decoder  

{

public:

NEMA_decoder();

virtual ~NEMA_decoder();

int add_m(const char * p, int num, 

pod_hvector<GPRMC_m>& pod_GPRMC, 

pod_hvector<GPGGA_m>& pod_GPGGA, 

pod_hvector<GPGSA_m>& pod_GPGSA, 

pod_hvector<GPGSV_m>& pod_GPGSV);

private:

decoder_e decode(GPRMC_m& GPRMC, 

GPGGA_m& GPGGA, 

GPGSA_m& GPGSA, 

pod_hvector<GPGSV_m>& pod_GPGSV,

pod_hvector<char>& m);

 

BOOL decode_GPGSV(pod_hvector<char>& m, pod_hvector<GPGSV_m>& m2);

BOOL decode_GPRMC(pod_hvector<char>& m, GPRMC_m& m2);

BOOL decode_GPGGA(pod_hvector<char>& m, GPGGA_m& m1);

BOOL decode_GPGSA(pod_hvector<char>& m, GPGSA_m& m1);

BOOL decode_INPUT(pod_hvector<char>& m, input_t* input, int num, int begin, int& end);

BOOL decode_VALUE(pod_hvector<char>& m, input_t& pod_input);

BOOL decode_CHECK(pod_hvector<char>& m);

pod_hvector<char> m_pod_buf;

};

//-------------------------------------------------cpp

NEMA_decoder::NEMA_decoder()

{

}

NEMA_decoder::~NEMA_decoder()

{

}

/*

逐字节判断

如果为ETX则合并前面剩余部分后解析

如果为HED则删除剩余部分

*/

int NEMA_decoder::add_m(const char * p, int num, 

pod_hvector<GPRMC_m>& pod_GPRMC, 

pod_hvector<GPGGA_m>& pod_GPGGA, 

pod_hvector<GPGSA_m>& pod_GPGSA, 

pod_hvector<GPGSV_m>& pod_GPGSV)

{

//------------------------------------------------------------debug

#if NEMA_debug

dec_loger.LogStr("recv[");

dec_loger.LogStr(p, num);

dec_loger.LogStr("]/r/n");

#endif

int ret = 0;

static pod_hvector<char> pod_m(200, 10);

for (int i = 0; i < num; i++)

{

char c = *(p+i);

if (c == HED)

{

pod_m.clear();

pod_m.add(c);

}

else if (c == ETX)

{

pod_m.add(c);

if(decode_CHECK(pod_m) == TRUE)

{

#if NEMA_debug

dec_loger.LogStr("decode[");

dec_loger.LogStr(pod_m.begin(), pod_m.size()-2);

dec_loger.LogStr("]/r/n");

#endif

GPRMC_m GPRMC;

GPGGA_m GPGGA; 

GPGSA_m GPGSA; 

  decoder_e state = decode(GPRMC,  GPGGA, GPGSA, pod_GPGSV, pod_m);

#if NEMA_debug

dec_loger.DumpVar("dec state[%d]/r/n", state);

#endif

ret |= state;

switch (state)

{

case GPRMC_e:

pod_GPRMC.add(GPRMC);

break;

case GPGSV_e:

break;

case GPGGA_e:

pod_GPGGA.add(GPGGA);

break;

case GPGSA_e:

pod_GPGSA.add(GPGSA);

break;

case NONE_e:

break;

default:

break;

}

}

#if NEMA_debug

else

{

dec_loger.LogStr("lost[");

if (pod_m.size()>2)

{

dec_loger.LogStr(pod_m.begin(), pod_m.size()-2);

}

else

{

dec_loger.LogStr(pod_m.begin(), pod_m.size());

}

dec_loger.LogStr("]/r/n");

}

#endif

pod_m.clear();

}

else

{

pod_m.add(c);

}

}

return ret;

}

decoder_e NEMA_decoder::decode(GPRMC_m& GPRMC, 

  GPGGA_m& GPGGA, 

  GPGSA_m& GPGSA, 

  pod_hvector<GPGSV_m>& pod_GPGSV,

  pod_hvector<char>& m)

{

char* p_end = m.end()-1;

char* p_begin = m.begin();

*p_end = '/0';//这里填入0是为了配合strstr

if (strstr(p_begin, "$GPRMC"))

{

if (decode_GPRMC(m, GPRMC) == TRUE)

return GPRMC_e;

}

else if (strstr(p_begin, "$GPGGA"))

{

if (decode_GPGGA(m, GPGGA) == TRUE)

return GPGGA_e;

}

else if (strstr(p_begin, "$GPGSA"))

{

if (decode_GPGSA(m, GPGSA) == TRUE)

return GPGSA_e;

}

else if (strstr(p_begin, "$GPGSV"))

{

if (decode_GPGSV(m, pod_GPGSV) == TRUE)

return GPGSV_e;

}

#if NEMA_debug

else

{

dec_loger.LogStr("LOST[");

dec_loger.LogStr(p_begin, m.size());

dec_loger.LogStr("]/r/n");

}

#endif

return NONE_e;

}

BOOL NEMA_decoder::decode_GPGSA(pod_hvector<char>& m1, GPGSA_m& m)

{

b_char_mem type;

b_char_mem utc_data;

input_t GPGSA_input [] = 

{

{STRING_e, &type},

{CHAR_e, &(m.mode1)},

{CHAR_e, &(m.mode2)},

{INT_e, &(m.channel[0])},

{INT_e, &(m.channel[1])},

{INT_e, &(m.channel[2])},

{INT_e, &(m.channel[3])},

{INT_e, &(m.channel[4])},

{INT_e, &(m.channel[5])},

{INT_e, &(m.channel[6])},

{INT_e, &(m.channel[7])},

{INT_e, &(m.channel[8])},

{INT_e, &(m.channel[9])},

{INT_e, &(m.channel[10])},

{INT_e, &(m.channel[11])},

{FLOAT_e, &(m.pdop)},

{FLOAT_e, &(m.hdop)},

{FLOAT_e, &(m.vdop)}

};

int end = 0;

int ret = decode_INPUT(m1, GPGSA_input, sizeof(GPGSA_input)/sizeof(input_t), 0, end);

return ret >= sizeof(GPGSA_input)/sizeof(input_t);

}

BOOL NEMA_decoder::decode_GPRMC(pod_hvector<char>& m1, GPRMC_m& m2)

{

  b_char_mem type;

b_char_mem mem_lat;

b_char_mem mem_lon;

input_t GPRMC_input [] = 

{

{STRING_e, &type},

{INT_e, &(m2.UTC_Time)},

{CHAR_e, &(m2.status)},

{STRING_e, &(mem_lat)},

{CHAR_e, &(m2.latitude)},

{STRING_e, &(mem_lon)},

{CHAR_e, &(m2.longtitude)},

{FLOAT_e, &(m2.speed)},

{FLOAT_e, &(m2.azimuth_angle)},

{INT_e, &(m2.UTC_Date)},

};

int end = 0;

int ret = decode_INPUT(m1, GPRMC_input, sizeof(GPRMC_input)/sizeof(input_t), 0, end);

if (ret < sizeof(GPRMC_input)/sizeof(input_t)) return TRUE;

int LLi = 0, LLf = 0;

sscanf(mem_lat.addr(), "%d.%d", &LLi, &LLf);

m2.latitude_value   = LLi/100  + double(double(LLi%100) + double(LLf)/100000.0)/60.0;

sscanf(mem_lon.addr(), "%d.%d", &LLi, &LLf);

m2.longtitude_value = LLi/100  + double(double(LLi%100) + double(LLf)/100000.0)/60.0;

return TRUE;

}

BOOL NEMA_decoder::decode_GPGGA(pod_hvector<char>& m, GPGGA_m& m2)

{

b_char_mem type;

b_char_mem mem_lat;

b_char_mem mem_lon;

input_t GPGGA_input [] = 

{

{STRING_e, &type},

{INT_e, &(m2.UTC_Time)},

{STRING_e, &(mem_lat)},

{CHAR_e, &(m2.lat)},

{STRING_e, &(mem_lon)},

{CHAR_e, &(m2.lon)},

{CHAR_e, &(m2.status)},

{INT_e, &(m2.sate_num)},

};

int end = 0;

int ret = decode_INPUT(m, GPGGA_input, sizeof(GPGGA_input)/sizeof(input_t), 0, end);

if (ret < sizeof(GPGGA_input)/sizeof(input_t)) return FALSE;

int LLi = 0, LLf = 0;

sscanf(mem_lat.addr(), "%d.%d", &LLi, &LLf);

m2.latValue = LLi/100  + double(double(LLi%100) + double(LLf)/100000.0)/60.0;

sscanf(mem_lon.addr(), "%d.%d", &LLi, &LLf);

m2.lonValue = LLi/100  + double(double(LLi%100) + double(LLf)/100000.0)/60.0;

return TRUE;

}

BOOL NEMA_decoder::decode_GPGSV(pod_hvector<char>& m, pod_hvector<GPGSV_m>& m2)

{

GPGSV_h_m m1;

b_char_mem type;

input_t GPGSV_h_input [] = 

{

{STRING_e, &type},

{INT_e, &(m1.gsvNum)},

{INT_e, &(m1.curId)},

{INT_e, &(m1.sateNum)},

};

//解析GPGSV头

int end = 0;

int ret = decode_INPUT(m, GPGSV_h_input, sizeof(GPGSV_h_input)/sizeof(input_t), 0, end);

if (ret < (sizeof(GPGSV_h_input)/sizeof(input_t))) return FALSE;

int nLoop = 0;

if (m1.curId == 1) m2.clear();

//分析此次卫星信息个数

if (m1.curId < m1.gsvNum) nLoop = 4;

else nLoop = m1.sateNum - (m1.curId - 1) * 4;

//循环

GPGSV_m m3;

input_t GPGSV_input [] = 

{

{INT_e, &(m3.sate_id)},

{INT_e, &(m3.degree1)},

{INT_e, &(m3.degree2)},

{INT_e, &(m3.ndb)},

};

for (int i = 0; i < nLoop; i++)

{

int ret = decode_INPUT(m, GPGSV_input, sizeof(GPGSV_input)/sizeof(input_t), end, end);

if (ret < (sizeof(GPGSV_input)/sizeof(input_t))) return FALSE;

m2.add(m3);

}

return m1.curId >= m1.gsvNum;

}

//解析每句

int NEMA_decoder::decode_INPUT(pod_hvector<char>& m, input_t* input, int num, int begin, int& end)

{

BYTE  CMANum = 0;

pod_hvector<char> pod_part(20,20);

if(num == 0) return 0;

for (int i = begin ; i < m.size(); i++)

{

if((m[i] >= 'a' && m[i] <= 'z') 

|| (m[i] >= 'A' && m[i] <= 'Z') 

|| (m[i] >= '0' && m[i] <= '9')

|| m[i] == '$'

|| m[i] == '.')

{

pod_part.add(m[i]);

}

else if (m[i] == CMA)

{

pod_part.add('/0');

decode_VALUE(pod_part, input[CMANum]);

pod_part.clear();

++CMANum;

if(CMANum >= num) 

{

++i;

break;

}

}

else if(m[i] == '/r' || m[i] == '*')

{

pod_part.add('/0');

decode_VALUE(pod_part, input[CMANum]);

++CMANum;

break;

}

else//基本不可能执行到这里

{

return 0;

}

}

end = i;

return CMANum;

}

//解析每个值,将得到的值赋值给特定类型的指针

BOOL NEMA_decoder::decode_VALUE(pod_hvector<char>& m, input_t& pod_input)

{

switch (pod_input.type)

{

case FLOAT_e:

*(float*)(pod_input.pointer) = atof(m.data());

break;

case INT_e:

*(int*)(pod_input.pointer) = atoi(m.data());

break;

case STRING_e:

*(b_char_mem*)(pod_input.pointer) << b_char_mem(m.data(), m.size());

break;

case CHAR_e:

*(char*)(pod_input.pointer) = *(m.data());

break;

}

return TRUE;

}

//GPS语句完整性效验

BOOL NEMA_decoder::decode_CHECK(pod_hvector<char>& m)

{

unsigned char check_SUM = 0;

for (int i = 0; i < m.size(); i++)

{

char c = m[i];

if (c == '$')

check_SUM = 0;

else if (c == '*')

break;

else

check_SUM ^= c;

}

if(i >= m.size()) return FALSE;

++i;

pod_hvector<char> pod_check_CHAR(3,3);

for (; i < m.size(); i++)

{

char c = m[i];

if(c == '/r' || c == '/n') break;

pod_check_CHAR.add(m[i]);

}

if(pod_check_CHAR.size() != 2) return FALSE;

pod_check_CHAR.add('/0');

unsigned char check_SUM1 = 0;

sscanf(pod_check_CHAR.data(), "%x", &check_SUM1);

return check_SUM1 == check_SUM;

}