motor/Common/middle/ParsePackUbx.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;
}