#include <iostream>
using namespace std;
/* Declation from CanModule.cpp */
uint32_t Endstop = 0xffffffff;
uint32_t tmpEndstopBits = 0xffffffff;
/* This does not seem to work */
//int testID = 1557;
/* All of these work*/
//int testID = 3;
int testID = 10;
//int testID = 17;
// Assumes little endian
void printBits(size_t const size, void const * const ptr)
{
unsigned char *b = (unsigned char*) ptr;
unsigned char byte;
int i, j;
for (i = size-1; i >= 0; i--) {
for (j = 7; j >= 0; j--) {
byte = (b[i] >> j) & 1;
printf("%u", byte);
}
}
puts("");
}
int main()
{
uint8_t pBuff[8];
pBuff[0] = (uint8_t)(testID >> 8);
pBuff[1] = (uint8_t)(testID);
pBuff[2] = 0; // 0 or 1
uint16_t MsgID;
uint16_t index;
MsgID = (uint16_t)((pBuff[0] << 8) | pBuff[1]);
cout << "Message ID: " << (uint16_t)MsgID << std::endl;
index = MsgID;
tmpEndstopBits |= (1 << index);
if(pBuff[2] == 0) tmpEndstopBits &= ~(1 << index);
cout << "tmpEndstopBits: ";
printBits(sizeof(tmpEndstopBits), &tmpEndstopBits);
Endstop &= ~0x7f; // clear bit
Endstop |= 0x7f & tmpEndstopBits & (tmpEndstopBits >> 7) & (tmpEndstopBits >> 14);
cout << "Endstop bits: ";
printBits(sizeof(Endstop), &Endstop);
return 0;
}
