int process_received_msg(uint8_t *payloadReceived) {
//Messages <= 255 start with 2. 2nd byte is length
//Messages >255 start with 3. 2nd and 3rd byte is length combined with 1st >>8 and then &0xFF
int counter = 0;
int endMessage = 256;
bool messageRead = false;
uint8_t messageReceived[256];
int lenPayload = 0;
while (SERIALIO.available()) {
messageReceived[counter++] = SERIALIO.read();
if (counter == 2) {//case if state of 'counter' with last read 1
switch (messageReceived[0])
{
case 2:
endMessage = messageReceived[1] + 5; //Payload size + 2 for sice + 3 for SRC and End.
lenPayload = messageReceived[1];
break;
case 3:
//ToDo: Add Message Handling > 255 (starting with 3)
break;
default:
break;
}
}
if (counter >= sizeof(messageReceived))
{
break;
}
if (counter == endMessage && messageReceived[endMessage - 1] == 3) {//+1: Because of counter++ state of 'counter' with last read = "endMessage"
messageReceived[endMessage] = 0;
#ifdef DEBUG
DEBUGSERIAL.println("End of message reached!");
#endif
messageRead = true;
break; //Exit if end of message is reached, even if there is still more data in buffer.
}
}
bool unpacked = false;
if (messageRead) {
unpacked = unpack_payload(messageReceived, endMessage, payloadReceived, messageReceived[1]);
}
if (unpacked)
{
return lenPayload; //Message was read
}
else {
return 0; //No Message Read
}
}
void test_unpack()
{
uint8_t packed_payload[] = {
0x02, 0x03, 0x00, 0x78, 0x46, 0x01, 0x3F, 0xBF, 0x03
};
uint8_t *payload;
uint16_t payload_length;
payload_length = unpack_payload(packed_payload, &payload);
TEST_CHECK(payload != NULL);
TEST_CHECK(payload_length == 3);
TEST_CHECK(payload[0] == 0x78);
TEST_CHECK(payload[1] == 0x46);
TEST_CHECK(payload[2] == 0x01);
free(payload);
}
void test_bijection()
{
uint8_t unpacked[] = {
0x06, 0x35, 0x33, 0x39, 0x54, 0x30, 0x39, 0x34, 0x35, 0x38, 0x32
};
unsigned int unpacked_length = 11;
uint8_t *packed;
uint16_t packed_length = pack_payload(unpacked, &packed, unpacked_length);
uint8_t *unpacked_result;
uint16_t unpacked_result_length = unpack_payload(packed, &unpacked_result);
TEST_CHECK(unpacked_result_length == unpacked_length);
int cmp = memcmp(unpacked, unpacked_result, unpacked_result_length);
TEST_CHECK(cmp == 0);
free(packed);
free(unpacked_result);
}
uint8_t unpack_payload_4(void)
{
return unpack_payload(rx_buf_4, rx_command_4);
}
uint8_t unpack_payload_3(void)
{
return unpack_payload(rx_buf_3, rx_command_3);
}
uint8_t unpack_payload_2(void)
{
return unpack_payload(rx_buf_2, rx_command_2);
}
uint8_t unpack_payload_1(void)
{
return unpack_payload(rx_buf_1, rx_command_1);
}
