119 lines
1.7 KiB
C
119 lines
1.7 KiB
C
/*
|
|
* ParserPack.c
|
|
*
|
|
* Created on: Jun 24, 2020
|
|
* Author: matth
|
|
*/
|
|
|
|
#include "ParsePackUbx.h"
|
|
#include "ublox.h"
|
|
|
|
void s_UbxCheckSum(uint8_t *buff, uint16_t len, uint8_t *CheckA, uint8_t *CheckB)
|
|
{
|
|
unsigned char cka=0,ckb=0;
|
|
int i;
|
|
|
|
for (i = 0; i < len; i++)
|
|
{
|
|
cka += buff[i];
|
|
ckb += cka;
|
|
}
|
|
|
|
if (NULL != CheckA)
|
|
{
|
|
*CheckA = cka;
|
|
}
|
|
if (NULL != CheckB)
|
|
{
|
|
*CheckB = ckb;
|
|
}
|
|
}
|
|
|
|
|
|
void ParserUbx_init(ParserUbx_t *parser)
|
|
{
|
|
parser->stage = 0;
|
|
}
|
|
|
|
int ParserUbx_char(ParserUbx_t *parser, uint8_t c)
|
|
{
|
|
int rslt = -1;
|
|
uint8_t s_checkA = 0;
|
|
uint8_t s_checkB = 0;
|
|
|
|
switch (parser->stage)
|
|
{
|
|
case 0:
|
|
if (c == 0xB5)
|
|
{
|
|
parser->stage++;
|
|
parser->buff[0] = c;
|
|
}
|
|
break;
|
|
case 1:
|
|
if (c == 0x62)
|
|
{
|
|
parser->stage++;
|
|
parser->buff[1] = c;
|
|
}
|
|
else
|
|
{
|
|
parser->stage = 0;
|
|
}
|
|
break;
|
|
case 2:
|
|
parser->msgClass = c;
|
|
parser->stage++;
|
|
parser->buff[2] = c;
|
|
break;
|
|
case 3:
|
|
parser->msgId = c;
|
|
parser->stage++;
|
|
parser->buff[3] = c;
|
|
break;
|
|
case 4:
|
|
parser->len = c;
|
|
parser->stage++;
|
|
parser->buff[4] = c;
|
|
break;
|
|
case 5:
|
|
parser->len += c<<8; //payload length
|
|
if (parser->len <=PARSER_UBX_BUFF_LEN)
|
|
{
|
|
parser->idx = 0;
|
|
parser->stage++;
|
|
parser->buff[5] = c;
|
|
}
|
|
else
|
|
{
|
|
parser->stage = 0;
|
|
}
|
|
break;
|
|
case 6:
|
|
parser->buff[6+parser->idx++] = c;
|
|
if (parser->idx == parser->len)
|
|
{
|
|
parser->stage++;
|
|
}
|
|
break;
|
|
case 7:
|
|
parser->checkA = c;
|
|
parser->stage++;
|
|
break;
|
|
case 8:
|
|
parser->checkB = c;
|
|
s_UbxCheckSum(&parser->buff[2], parser->len+4, &s_checkA, &s_checkB);
|
|
if((parser->checkA == s_checkA) && (parser->checkB == s_checkB))
|
|
{
|
|
rslt = parser->msgClass;
|
|
}
|
|
parser->stage = 0;
|
|
break;
|
|
default:
|
|
parser->stage = 0;
|
|
break;
|
|
}
|
|
|
|
return rslt;
|
|
}
|