gcs-nf/Plugins/ToolsUI/LandingGear_AJ500/ParsePack.c

/*
 * ParserPack.c
 *
 *  Created on: Jun 24, 2020
 *      Author: matth
 */

#include "ParsePack.h"

static uint16_t CRC_CheckSum (uint8_t *pBuffer, uint16_t len)
{
	uint16_t poly = 0x8408;
	uint16_t crc = 0;
	uint8_t carry;
	uint8_t i_bits;
	uint16_t j;

	for (j = 0; j < len; j++) {
		crc = (uint16_t) (crc ^ (uint8_t) pBuffer[j]);
		for (i_bits = 0; i_bits < 8; i_bits++) {
			carry = (uint8_t) (crc & 1);
			crc = (uint16_t) (crc / 2);
			if (carry) {
				crc = (uint16_t) (crc ^ poly);
			}
		}
	}
	return crc;
}


void Packer_init(Packer_t *pack)
{
	pack->buff[0] = 0xEB;
	pack->buff[1] = 0x90;
	pack->seq = 0;
}

size_t Packer_pack(Packer_t *pack, uint8_t id, uint8_t pkg[], uint16_t len)
{
	uint16_t j;
	size_t i;

	pack->buff[2] = pack->seq++;
	pack->buff[3] = id;
	pack->buff[4] = (len >> 8) & 0xFF;
	pack->buff[5] = len & 0xFF;
	for (i=6,j=0u;j<len;++i,++j)
	{
		pack->buff[i] = pkg[j];
	}
	uint16_t crc16 = CRC_CheckSum(&pack->buff[6], len);
	pack->buff[i++] =  (crc16 & 0xFF);
	pack->buff[i++] =  (crc16 >> 8)&0xFF;
	return i;
}

void Parser_init(Parser_t *parser)
{
  parser->stage = 0;
  parser->crc_err_cnt = 0;
  parser->head_err_cnt = 0;
}

int Parser_char(Parser_t *parser, uint8_t c)
{
	int rslt = -1;

	switch (parser->stage)
	{
	case 0:
		if (c == 0xEB)
		{
			parser->stage++;
		}
		else
		{
			parser->head_err_cnt++;
		}
		break;
	case 1:
		if (c == 0x90)
		{
			parser->stage++;
		}
		else
		{
			parser->stage = 0;
			parser->head_err_cnt++;
		}
		break;
	case 2:
		parser->seq = c;
		parser->stage++;
		break;
	case 3:
		parser->id = c;
		parser->stage++;
		break;
	case 4:
		parser->len = c;
		parser->stage++;
		break;
	case 5:
		parser->len = (parser->len << 8) + c;
		if (parser->len <=PARSER_BUFF_LEN)
		{
		  parser->idx = 0;
		  parser->stage++;
		}
		else
		{
		  parser->stage = 0;
		}
		break;
	case 6:
		parser->buff[parser->idx++] = c;
		if (parser->idx == parser->len)
		{
			parser->stage++;
		}
		break;
	case 7:
		parser->crc16 = c;
		parser->stage++;
		break;
	case 8:
		parser->crc16 += (c<<8);
		if (parser->crc16 == CRC_CheckSum(parser->buff, parser->len))
		{
			rslt = parser->id;
		}
		else
		{
			parser->crc_err_cnt++;
		}
		parser->stage = 0;
		break;
	default:
		parser->stage = 0;
		break;
	}

	return rslt;
}

void Packer2_init(Packer2_t *pack)
{
	pack->buff[0] = 0xEB;
	pack->buff[1] = 0x90;
}

size_t Packer2_pack(Packer2_t *pack, uint8_t id, uint8_t pkg[], uint8_t len)
{
	uint8_t j;
	size_t i;
	uint8_t sum;

	pack->buff[2] = id;
	pack->buff[3] = len;
	sum = 0u;
	for (i=4,j=0u;j<len;++i,++j)
	{
		pack->buff[i] = pkg[j];
	    sum += pkg[j];
	}
	pack->buff[i++] = sum;
	return i;
}

void Parser2_init(Parser2_t *parser)
{
  parser->stage = 0;
}


int Parser2_char(Parser2_t *parser, uint8_t c)
{
	int rslt = -1;

	switch (parser->stage)
	{
	case 0:
		if (c == 0xEB)
		{
			parser->stage++;
		}
		break;
	case 1:
		if (c == 0x90)
		{
			parser->stage++;
		}
		else
		{
			parser->stage = 0;
		}
		break;
	case 2:
		parser->id = c;
		parser->stage++;
		break;
	case 3:
		parser->len = c;
		parser->stage++;
		parser->idx = 0;
		parser->sum = 0;
		break;
	case 4:
		parser->buff[parser->idx++] = c;
		parser->sum += c;
		if (parser->idx == parser->len)
		{
			parser->stage++;
		}
		break;
	case 5:
		if (parser->sum == c)
		{
			rslt = parser->id;
		}
		parser->stage = 0;
		break;
	default:
		parser->stage = 0;
		break;
	}

	return rslt;
}