155 lines
3.3 KiB
C
155 lines
3.3 KiB
C
/*
|
|
* ParserPack.c
|
|
*
|
|
* Created on: Jun 24, 2020
|
|
* Author: matth
|
|
*/
|
|
|
|
#include "ParsePackNovatel.h"
|
|
|
|
|
|
|
|
void ParserNovatel_init(ParserNovatel_t *parser_novatel)
|
|
{
|
|
parser_novatel->stage = 0;
|
|
}
|
|
|
|
unsigned int gps_crc32(const unsigned char *buff, int len)
|
|
{
|
|
#define POLYCRC32 (0xEDB88320u)
|
|
|
|
unsigned int crc=0;
|
|
int i,j;
|
|
|
|
for (i=0; i<len; i++)
|
|
{
|
|
crc^=buff[i];
|
|
for (j=0; j<8; j++)
|
|
{
|
|
if (crc&1) crc=(crc>>1)^POLYCRC32;
|
|
else crc>>=1;
|
|
}
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
|
|
/** 解析Novatel类的RTK协议部分 */
|
|
|
|
#define SUSPEND_STEP_FIRST 1 //需要暂停的字节数-1
|
|
#define SUSPEND_STEP_SECOND 17 //需要暂停的字节数-1
|
|
|
|
int ParserNovatel_char(ParserNovatel_t *parser_novatel, uint8_t c)
|
|
{
|
|
int rslt = -1;
|
|
|
|
switch (parser_novatel->stage)
|
|
{
|
|
case 0:
|
|
if (c == 0xAA) //RTK帧头0xAA4412
|
|
{
|
|
parser_novatel->stage++;
|
|
parser_novatel->buff[0] = c;
|
|
}
|
|
else
|
|
{
|
|
parser_novatel->head_err_cnt ++;
|
|
}
|
|
break;
|
|
case 1:
|
|
if (c == 0x44)
|
|
{
|
|
parser_novatel->stage++;
|
|
parser_novatel->buff[1] = c;
|
|
}
|
|
else
|
|
{
|
|
parser_novatel->stage = 0;
|
|
}
|
|
break;
|
|
case 2:
|
|
if (c == 0x12)
|
|
{
|
|
parser_novatel->stage++;
|
|
parser_novatel->buff[2] = c;
|
|
}
|
|
else
|
|
{
|
|
parser_novatel->stage = 0;
|
|
}
|
|
break;
|
|
case 3:
|
|
parser_novatel->head_len = c;
|
|
parser_novatel->stage++;
|
|
parser_novatel->buff[3] = c;
|
|
break;
|
|
case 4:
|
|
parser_novatel->id = c;
|
|
parser_novatel->stage++;
|
|
parser_novatel->buff[4] = c;
|
|
break;
|
|
case 5:
|
|
parser_novatel->buff[5] = c;
|
|
parser_novatel->id += c<<8;
|
|
parser_novatel->stage++;
|
|
parser_novatel->suspend_step = 0;
|
|
break;
|
|
|
|
case 6: //有保留位 需要暂停一下
|
|
parser_novatel->buff[6+parser_novatel->suspend_step] = c;
|
|
if(++parser_novatel->suspend_step >= SUSPEND_STEP_FIRST+1) //这里选择+1是为了前面parser->buff[5]中的数字是每个case递增的
|
|
{
|
|
parser_novatel->stage++;
|
|
parser_novatel->suspend_step = 0;
|
|
}
|
|
break;
|
|
case 7:
|
|
parser_novatel->body_len = c;
|
|
parser_novatel->stage++;
|
|
parser_novatel->buff[7+SUSPEND_STEP_FIRST] = c;
|
|
break;
|
|
case 8:
|
|
parser_novatel->body_len += c<<8;
|
|
if (parser_novatel->body_len <=PARSER_BUFF_LEN)
|
|
{
|
|
parser_novatel->idx = 0;
|
|
parser_novatel->stage++;
|
|
}
|
|
else
|
|
{
|
|
parser_novatel->stage = 0;
|
|
}
|
|
parser_novatel->buff[8+SUSPEND_STEP_FIRST] = c;
|
|
parser_novatel->suspend_step = 0;
|
|
break;
|
|
case 9: //有保留位 需要暂停一下
|
|
parser_novatel->buff[9+SUSPEND_STEP_FIRST+parser_novatel->suspend_step] = c;
|
|
if(++parser_novatel->suspend_step >= SUSPEND_STEP_SECOND+1)
|
|
{
|
|
parser_novatel->stage++;
|
|
parser_novatel->suspend_step = 0;
|
|
}
|
|
break;
|
|
case 10:
|
|
parser_novatel->buff[10+SUSPEND_STEP_FIRST+SUSPEND_STEP_SECOND+parser_novatel->idx++] = c;
|
|
if (parser_novatel->idx == parser_novatel->body_len+4) //4 bit crc
|
|
{
|
|
parser_novatel->stage = 0; //解析到完整包长度 状态机归位
|
|
|
|
parser_novatel->crc32 = parser_novatel->buff[parser_novatel->body_len+28] | parser_novatel->buff[parser_novatel->body_len+28+1]<<8 | parser_novatel->buff[parser_novatel->body_len+28+2]<<16 | parser_novatel->buff[parser_novatel->body_len+28+3]<<24;
|
|
parser_novatel->crc_cal = gps_crc32(parser_novatel->buff, parser_novatel->body_len+28);
|
|
|
|
if (parser_novatel->crc_cal == parser_novatel->crc32)
|
|
{
|
|
rslt = parser_novatel->id; //解析到完整包+校验通过 返回包ID
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
parser_novatel->stage = 0;
|
|
break;
|
|
}
|
|
|
|
return rslt;
|
|
}
|