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
38
39

#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);
}