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

#include "ibus.h"

uint16_t CRC16Gen(*IBusFrame frame){
	uint16_t checksum = 0xFFFF;
	for (int i = 2; i < frame[0]-2; i++) {
		checksum -= ibusPacket[i];
	}

	return checksum;
}

int encodeIBus(*IBusFrame frame, *channelValues vals){
	*frame=IBusSize;
	*(frame+1)=startByteBeta;
	for (int i=0; i<channelC*2; i++){
		if (i%2)
			*(frame+i+2)=(uint8_t) (*(vals+(i/2)))>>sizeof(uint8_t);
		else
			*(frame+i+2)=(uint8_t) (*(vals+(i/2)) & 0xFF);
	}
	uint16_t checksum=CRC16Gen(frame);
	*(frame+frame[0]-2)=(uint8_t)(checksum & 0xFF)
	*(frame+frame[0]-1)=(uint8_t)(checksum>>8)
	
}