ibus.c


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

#include "ibus.h"

int encodeIBus(*IBusFrame frame, *channelValues vals){
	*frame=startByteAlpha;
	*(frame+1)=startByteAlpha;
	for (int i=0; i<channelC*2; i++){
		if (i%2)
			*(frame+i+2)=(char) (*(vals+(i/2)))>>sizeof(char);
		else
			*(frame+i+2)=(char) *(vals+(i/2));
	
	
}

int CRC16Gen(*IBusFrame frame){
	unsigned char i;
	unsigned int data;
	unsigned int crc = 0xffff;

	if (length == 0)
		return (~crc);

	do{
		for (i=0, data=(unsigned int)0xff & *data_p++;i < 8; i++, data >>= 1){
			if ((crc & 0x0001) ^ (data & 0x0001))
				crc = (crc >> 1) ^ POLY;
			else
				crc >>= 1;
		}
	} while (--length);

	crc = ~crc;
	data = crc;
	crc = (crc << 8) | (data >> 8 & 0xff);

	return (crc);
}