Arduino Mega and fast serial communication

I need a bit advice with the following topic: fast serial communication between two Mega (2560) boards.

Made a simple test to ensure that it works. Baud rate can be set to 500k without issue. 1M started causing issues but 500k is fine. The transmission duration doesn't seem to be too long. It's important to send the data quickly so that the controller can get back to the main thing which is periodic controls at 10ms level if possible (most likely I need to slow it down to 30ms but that is still fine).

But when I made a first proper version I started encountering strange issues. One board was sending 48bytes of data (data structure) which looked correct in the oscilloscope (decoded the data and looked fine) but the receiving party started to misbehave. Oftentimes rebooted, data was corrupt from the beginning and end and was changing in a strange way. Slowed the speed but made no difference. Increased the execution period at receiving end to made sure nothing gets missed but no help.

Them started testing with a shorter amount of data. 16 bytes ==> no problem. 32 bytes ==> no problem. So, apparently the issue is with the data. Most likely the amount of data will increase by several bytes still later.

To my understanding the buffer should be 64bytes but I'm not sure.

Question is that should I start making a simple protocol to transfer data in blocks (send package 1/n, ACK, ... send package n/n, ACK) or is there a better alternative available?

I don't need error correction for the data. It's simply overwritten in the next cycle thus doesn't really matter if a message is broken.

TIA

I assume the data is in binary?
how do you indicate the start (and end unless it is a fixed size) of the data?
do you have a checksum or CRC check?
I would tend to create a frame with start and end sequences plus CRC check and byte stuff the data

Your long descriptions tell me you are NOT reading and doing something with each byte as you find it in the buffer. Until you actually do that you will have NO control over the "data speed".

Data is binary and fixed structure.

Currently no CRC.

Original idea was just to send data with Serial.write and receive with Serial.readBytes with a fixed number of bytes.

Did some further testing and turns out that even 32 bytes is too long data block. Sometimes the data gets "out of sync". With 16 bytes at a time I have not seen any issues yet.

No action per byte. Simply just receive X number of bytes (originally 48 which is the current size of the data structure) sent by the other party. After that data is simply moved from message buffer to internal variables.

Data is sent periodically which is way slower that the actual sending. Currently every 20ms and the transmission takes 113us (no fluctuation of the transmission duration). The assumption I have is that receiving party takes all bytes as they have sent and usually this works just fine. Occasionally I see some transmission errors (bytes are offset).

Reboots might indicate writing outside the boundaries of arrays. I suggest that you post both the sender and the receiver code so people can check.

Sending part:

#include "local.h"
#include <HeliOS.h>
#include "inter_comm.h"


// Receive message from Arduino CTRL
struct
{
	// 32bit elements
	// Left
	f32	left_measSpeed;
	f32 left_measSpeed_ratio;
	f32 left_speed_ratio;
	f32 left_newPWM_ratio;
	// Right
	f32	right_measSpeed;
	f32 right_measSpeed_ratio;
	f32 right_speed_ratio;
	f32 right_newPWM_ratio;

	// 16bit elements
	i16 stepPos;

	// 8bit elements
	u8	lStatus;
	u8	pStatus;

	// ** DEVELOPMENT RELATED **
//	u8	padding;				// Needed for 32bit alignment
	Dev_data_st	DevData;		// Development data (for trending with Teleplot etc.)

} messageToArduinoCOMM;
#define MESSAGE_TO_COMM_SIZE sizeof(messageToArduinoCOMM)

// ** DEVELOPMENT DATA STRUCTURE **
typedef  struct 
{
	u32	cyclePeriod;
	f32 left_ref;
	f32 right_ref;
} Dev_data_st;
#define DEV_DATA_SIZE sizeof(Dev_data_st)


// ** DEVELOPMENT DATA **
Dev_data_st DevData;


// - Communicate with controller
void TaskInterCommunication(xTask task_, xTaskParm parm_)
{
	u8 bytes;

	// Send messageToArduinoCOMM as one message
	// - copy data from Modbus/RTU buffer to message buffer
	// - send data to COMM Arduino
	messageToArduinoCOMM.left_measSpeed        = 1;
	messageToArduinoCOMM.left_measSpeed_ratio  = 2;
	messageToArduinoCOMM.left_speed_ratio      = 3;
	messageToArduinoCOMM.left_newPWM_ratio     = 4;

	messageToArduinoCOMM.right_measSpeed       = 5;
	messageToArduinoCOMM.right_measSpeed_ratio = 6;
	messageToArduinoCOMM.right_speed_ratio     = 7;
	messageToArduinoCOMM.right_newPWM_ratio    = 8;

	messageToArduinoCOMM.stepPos     = 9;
	messageToArduinoCOMM.liftStatus  = 10;
	messageToArduinoCOMM.pedalStatus = 11;

	// ** DEVELOPMENT PURPOSES **
	memcpy(&messageToArduinoCOMM.DevData, &DevData, DEV_DATA_SIZE);
	//messageToArduinoCOMM.DevData.cyclePeriod = 5;

//	bytes = Serial2.write((const char *)&messageToArduinoCOMM, MESSAGE_TO_COMM_SIZE);
	bytes = Serial2.write((const char *)&messageToArduinoCOMM, 16);
	Serial.print(F("Sent (bytes): ")); Serial.println(bytes);

	// Receive messageToArduinoCOMM if there's data
	bytes=0;
	if (Serial2.available())
	{
		while (Serial2.available())
		{
			// Read all data
			bytes = Serial2.readBytes((byte *)&messageToArduinoCTRL, MESSAGE_TO_CTRL_SIZE);
		}
		if (bytes != MESSAGE_TO_CTRL_SIZE)
		{
			// Error: not enough data
			Serial.print(F("Incomplete message from CTRL (received / message size bytes): ")); Serial.print(bytes); Serial.print(F(" / ")); Serial.println(MESSAGE_TO_CTRL_SIZE);
		}
		else
		{
			// Message received OK ==> copy data from message buffer to internal variables


			
		}
	}
}


Receiving part:

#include <HeliOS.h>
#include <FastCRC.h>
#include "inter_comm.h"
#include "motor_comm.h"

// Receive message from Arduino CTRL
struct
{
	// 32bit elements
	// Left
	f32	left_measSpeed;
	f32 left_measSpeed_ratio;
	f32 left_speed_ratio;
	f32 left_newPWM_ratio;
	// Right
	f32	right_measSpeed;
	f32 right_measSpeed_ratio;
	f32 right_speed_ratio;
	f32 right_newPWM_ratio;

	// 16bit elements
	i16 stepPos;

	// 8bit elements
	u8	lStatus;
	u8	pStatus;

	// ** DEVELOPMENT RELATED **
//	u8	padding;				// Needed for 32bit alignment
	Dev_data_st	DevData;		// Development data (for trending with Teleplot etc.)

} messageToArduinoCOMM;
#define MESSAGE_TO_COMM_SIZE sizeof(messageToArduinoCOMM)

// ** DEVELOPMENT DATA STRUCTURE **
typedef  struct 
{
	u32	cyclePeriod;
	f32 left_ref;
	f32 right_ref;
} Dev_data_st;
#define DEV_DATA_SIZE sizeof(Dev_data_st)

// ** DEVELOPMENT DATA **
Dev_data_st DevData;

// - Communicate with controller
void TaskInterCommunication(xTask task_, xTaskParm parm_)
{
	u8 bytes;

	// Receive messageToArduinoCOMM if there's data
	bytes=0;
	if (Serial2.available())
	{
		while (Serial2.available())
		{
			// Read all data
//			bytes = Serial2.readBytes((byte *)&messageToArduinoCOMM, MESSAGE_TO_COMM_SIZE);
			bytes = Serial2.readBytes((byte *)&messageToArduinoCOMM, 16);
		}
//		if (bytes != MESSAGE_TO_COMM_SIZE)
		if (bytes != 16)
		{
			// Error: not enough data
			Serial.print(F("Incomplete message from CTRL (received / message size bytes): ")); Serial.print(bytes); Serial.print(F(" / ")); Serial.println(MESSAGE_TO_COMM_SIZE);
		}
		else
		{
			// Message received OK ==> copy data from message buffer to internal variables
			//Serial.print(F("Bytes received: "));  Serial.println(bytes);

			// ** COPY DEVELOPMENT DATA **
			memcpy(&DevData, &messageToArduinoCOMM.DevData, DEV_DATA_SIZE);

			
		}
	}


	u8 *buffer = (u8 *)&messageToArduinoCOMM;
	for (u8 i=0; i < MESSAGE_TO_COMM_SIZE; i++)
	{
		Serial.print(*buffer++); Serial.print(F(" "));
	}
	Serial.println();
}

Simplified version but shows the essence of the code. Sending 16bytes and receiving 16bytes works almost all the time fine. Anything above than that starts to fail.

No more than specified number of bytes are received. Otherwise error is printed and that doesn't happen. Memory overflow/overwriting is hardly the case - at least with the code I've written. Dunno about the serial port related code.

Not much to say about the sender prg except you only check Serial2.available() after having sent the data which is every 20ms or so. It should have its own routine in the main loop.

On the receiver side, it's easy to be out of sync : if, for whatever reason, one byte is missing, the Serial2.readBytes((byte *)&messageToArduinoCOMM, MESSAGE_TO_COMM_SIZE); will wait until the next frame (the default Serial.setTimeout() being 1000ms) to fill the structure (with what will appear as garbage).

I would set the Serial.setTimeout() to, say, half the frames spacing and check :

  if (Serial2.available() >= MESSAGE_TO_COMM_SIZE)
  {
    bytes = Serial2.readBytes((byte *)&messageToArduinoCOMM, MESSAGE_TO_COMM_SIZE);
  }
  if (Serial2.available())
  {
    // Out of sync with the sender, discard data
    defaultValues(&messageToArduinoCOMM);
    // and empty the receiving buffer
    delayMicroseconds(ONE_TRANSMISSION_LENGTH_113us); // <- optional or safe?
    while (Serial2.available())
    {
      int trash = Serial2.read();
    }
    // Rx2 buffer is now empty and ready for a new message
  }

EDIT : smarter data usage :

if (Serial2.available() >= MESSAGE_TO_COMM_SIZE)
{
  bytes = Serial2.readBytes((byte *)&temporaryBuffer, MESSAGE_TO_COMM_SIZE);
}
if (Serial2.available())
{
  // Out of sync with the sender, keep old data
  // and empty the receiving buffer
  delayMicroseconds(ONE_TRANSMISSION_LENGTH_113us); // <- optional or safe?
  while (Serial2.available())
  {
    int trash = Serial2.read();
  }
  // Rx2 buffer is now empty and ready for a new message
}
else
{
  // update the structure
  memcpy(&temporaryBuffer, &messageToArduinoCOMM, MESSAGE_TO_COMM_SIZE);
}

Timing analysis.
500kbaud - 500k bits per second.
16 bytes = 160 bits(because at a minimum, 1 start and 1 stop bit per byte).
160 bits at 500kb/s would take 320 us. So I don't know what you're looking at with your 'scope, but it isn't a complete transmission in 113 us.
Rethink.

I think that your problem is that you assume that, when you have received one byte, you have received all bytes.

That is not the case. A byte might still be in transmission from the sender and your while() will end. Next time that your reading starts, you start with that last byte that by now is received.

You should read till your byte count is 48 and next process the data. Although Robin's updated Serial Input Basics is text based, example 3 and example 5 demonstrate how to receive till you have a full packet.

A 48 byte packet @500 kBaud takes roughly 1 millisecond to be transmitted. If I understand you correctly, you want to send every 10 ms. You will easily be able to add a timeout mechanism to your receiver.

Below program demonstrates the basics to give you the idea; it might need some fine tuning. Note that it uses Serial3 on the Mega so you need to replace Serial3 by Serial2 for your use case.

// status of receive operation
enum RXSTATUS
{
  IDLE,
  INPROGRESS,
  COMPLETE,
  TIMEOUT,
};

// buffer to store received data
uint8_t rxBuffer[48];
// receive timeout value
const uint32_t rxTimeout = 20;

void setup()
{
  Serial.begin(115200);
  Serial.println(F("Mega receiver 0.1"));
  Serial3.begin(500000);
}

void loop()
{
  // status of receive
  RXSTATUS status = receive();
  // previous status of receive for state change
  static RXSTATUS prevStatus = IDLE;

  // only display changes in receive status
  if (status != prevStatus)
  {
    Serial.print(F("Status changed from "));
    Serial.print(prevStatus);
    Serial.print(F(" to "));
    Serial.println(status);

    prevStatus = status;
  }

  // if a comnplete packet is received
  if (status == COMPLETE)
  {
    Serial.print(sizeof(rxBuffer));
    Serial.println(F(" bytes received"));
    for (uint8_t cnt = 0; cnt < sizeof(rxBuffer); cnt++)
    {
      if (rxBuffer[cnt] <= 0x10)
        Serial.print(F("0"));
      Serial.print(rxBuffer[cnt], HEX);
      Serial.print(F(" "));
    }
    Serial.println();
  }
}

/*
Receive on Serial3
Returns:
  status
*/
RXSTATUS receive()
{
  // status of receive
  static RXSTATUS rxStatus = IDLE;
  // index in receive buffer
  static uint8_t index;
  // time when last byte was received
  static uint32_t rxTime;

  // if receive in progress
  if (rxStatus == INPROGRESS)
  {
    // check for timeout
    if (millis() - rxTime > rxTimeout)
    {
      // reset index
      index = 0;
      // update status to idle
      rxStatus = IDLE;

      // indicate to caller that receive has timed out
      return TIMEOUT;
    }
  }

  while (Serial3.available())
  {
    // time of last byte rceived
    rxTime = millis();
    rxBuffer[index++] = Serial3.read();
    // change status to INPROGRESS
    rxStatus = INPROGRESS;
    // if full packet received
    if (index == sizeof(rxBuffer))
    {
      // reset index
      index = 0;
      // change status to COMPLETE
      rxStatus = IDLE;
      // indicate to caller that receive is complete
      return COMPLETE;
    }
  }

  // return the status
  return rxStatus;
}

I did write an elementary sender that can force a time out by setting an inter character delay.

Typical output if a full packet is received

09:13:51.921 -> Status changed from 0 to 1
09:13:51.921 -> Status changed from 1 to 2
09:13:51.921 -> 48 bytes received
09:13:51.962 -> 00 01 02 03 04 05 06 07 08 09 0A 0B 0C 0D 0E 0F 010 11 12 13 14 15 16 17 18 19 1A 1B 1C 1D 1E 1F 20 21 22 23 24 25 26 27 28 29 2A 2B 2C 2D 2E 2F 

Typical output with a timeout (icd = 25 ms)

09:15:00.613 -> Status changed from 0 to 1
09:15:00.644 -> Status changed from 1 to 3
09:15:00.644 -> Status changed from 3 to 0

// Edit
My sender only sends every second; this was done so the receiving Mega has time to print; I actually should have tried 2000000 baud rate for Serial on the Mega.

But I'm 99.99% confident that this will work correctly with limited printing.

Didn't calculate the speed in time. Just put the rules in place where the transmission started and where it ended. Data was decoded correctly as well so it wasn't broken.

The code actually read all 48 bytes at a time (Serial2.readBytes(buffer,48) ) because it was known by both parties how much data will be sent (single structure only).

Did some further development yday to split up the message to max 16 bytes parts and noticed some quite odd behavior that explains some things. First of all, it's pretty much always out of sync. If a communication glitch occur, it's "permanently" out-of-sync.

I tried to solve this by reading out everything from serial line so that next time when new data comes, it'll be correct (added simple header to the beginning of package to identify valid messages). Didn't work. This reading actually read way more data than there should have been (reading byte by byte). 60bytes of data whereas there should have been less than 16. There was valid info but there was also repeated info.

Didn't have time to put more time to figure out what is actually going on. It became pretty clear that I need to add simple negotiation to the communication to get rid of these issues. Simple Start<>ACK, package 1<>ACK, ..., package n<>ACK (last ACK completes the transmission of one message). NACK fails transmission and either re-transmit the package or just ignore and send whole message again after 10..20..30ms.

Unfortunately I cannot put this communication to the main loop. This is purely secondary function of the controller and I can easily sacrifice the speed later since it's for visualization purposes. But for now, I need it for controls development and controls are running as fast as possible. 10ms is most likely not doable. 30ms is still fine, probably even 50ms is still OK. PID (if I can actually use it) tuning becomes quite tricky if the transmission of controls related data (references, setpoints, measurements..) is too slow.

Yes. Out-of-sync became an issue almost immediately. I assumed that this can be solved with a fast communication, clearing trash out from serial port if something goes wrong and then be ready for new message. Receiving part is running quite much other code (display related + few measurements, Modbus communication..) thus receiving can be slowed down too much at times causing issues.

One additional Q related to this. Examples you've provided have been using reading one byte at a time. In this simple protocol I'm writing, I know exactly how many bytes can be expected in the transmission of a packet thus wouldn't it be more CPU efficient to use readBytes function with specified number of bytes than looping with single byte read-function?

That's my opinion too, and I read the bytes only when they are all 48 available in the serial buffer thus I don't have to loop with a while()

Is it possible to get interrupt from serial port? This would speed up communication massively because now I have to wait for response from the other party for the initial command.

Each time a new byte is received, an interrupt is generated and that byte is added to the 64 bytes buffer. Serial does that for you but you can probably stop using serial and set the relevant registers of the microcontroller to trigger your own ISR. Not the easiest thing in the world but doable I suppose. Not sure it's a huge advantage over the non blocking if(serial.available()...) in the loop() though...
EDIT : the one you would actually stop using is Serial2.
Serial2.begin() does just that : setting the (numerous) relevant registers to use the second serial port on the appropriate pins at the requested baud rate and set the 'Rx byte full' interrupt vector to point to its serial Rx ISR (the one you want to modify) as well as 'Tx Byte empty' to point towards the Tx ISR and probably a few other things.

2 Likes

hmm.. too complex. I was looking for a simple solution where I wouldn't have to wait at receiving end when there's something available. HeliOS is not as good as FreeRTOS which would have allowed to have a dedicated task just for this but I cannot use FreeRTOS with SPI connected display with Adafruit libs and Modbus/RTU master lib.

Well, need to make a small dedicated task that is executed often and just waits for new data. HeliOS doesn't allow anything to be placed in loop().

Finally made a simple protocol to transmit up to 255 bytes of structure in multiple parts (16 bytes per part). Added flow control to get rid of sync issues. All fine except it's slow. 48 bytes transmission can be repeated every 150ms or timeouts starts to hit. For display purposes this is still fine but for controls development most likely not.

Tempted to make a parallel communication using DIO channels which I have plenty of left (due using two megas). There's interrupt pins available on receiving board still which would make the response to sender very quick. 4 or 8 bits at a time + couple of write signals (bi-directional communication). Board is so full that it's not easy to find space for resistors anymore..

Here's the protocol code, if anyone is interested. Most likely works only in one direction which is sufficient for me for now.

// Multi-part message transmission
//
// Protocol:
// - communication needs handshaking to ensure transmission is in sync
// 		- Sender: 2 message types (Start transmission, Payload data)
//		- Receiver: 1 message type (number of received bytes including header)
//
// Sender: Start transmission
// - byte 0: Command
//		- 4bits: reserved
//		- 4bits: Command (1: New message)
// - byte 1: Payload length (1-255 bytes)
// - byte 2: CRC8
//
// Sender: Payload data
// - byte 0: Command
//		- 4bits: reserved
//		- 4bits: Command (2: Next package, 3: Cancel transmission)
// - byte 1-15: payload
//
// Receiver: Response 
// - byte 0: Number of received bytes (including header)
//
#define MSG_LEN 	16								// Max message length including header
#define MSG_PAYLOAD (MSG_LEN - 1)					// Payload length
#define TIMEOUT		10								// Timeout in ms for receiving response
#define TIMEOUT_NEW	20								// Timeout in ms for receiving response to New message command (waiting for receiving end to pick up)
u8 s_package[MSG_LEN];
FastCRC8 sendCRC8;

// Send message (single or multi-part)
u8 sendMessage(u8 *msg, u8 len)
{
	u8 payloadLen, bytes, totalLen;

	// Start transmission by sending a command
	s_package[0] = 0x01;							// Command (1: New message)
	s_package[1] = len;								// Payload length
	s_package[2] = sendCRC8.smbus(msg, len);		// CRC8 SMBUS

	bytes = Serial2.write(s_package, 3);			// Send command

	if (bytes != 3)
	{
		// Error sending message
		Serial.print(F("New message failed (sent / total): ")); Serial.print(bytes); Serial.print(F(" / ")); Serial.println(len);
		return 255;
	}

	// Wait for response
	switch (waitForResponse(3, TIMEOUT_NEW))
	{
		case 255:
			// Timeout
			Serial.print(F("Timeout to New message command: ")); Serial.println((u32)*msg);
			return 255;
			break;

		case 254:
			// Invalid response
			Serial.println(F("Invalid response to New message command."));
			return 255;
			break;
		
		case 0:
		default:
			// Response fine --> continue to payload transmission
			break;
	}

	// Command successfully transmitted. Now send the payload.
	// - check if message needs to be sent as a single message or as multi-part message
	if (len <= MSG_PAYLOAD)
	{
		// Single message only --> Send command (2: Next package) byte first and then payload
		Serial2.write(2);

		// Send message
		bytes = Serial2.write(msg, len);

		if (bytes != len)
		{
			// Error sending message
			Serial.print(F("Message sending failed (sent / total): ")); Serial.print(bytes); Serial.print(F(" / ")); Serial.println(len);
			return 255;
		}

		// Wait for response
		switch (waitForResponse(len + 1, TIMEOUT))			// Length: Command + Payload
		{
			case 255:
				// Timeout
				Serial.print(F("Timeout to single Payload: ")); Serial.println((u32)*msg);
				return 255;
				break;

			case 254:
				// Invalid response
				Serial.println(F("Invalid response to single Payload data command."));
				return 255;
				break;
			
			case 0:
			default:
				// Response fine --> Payload correctly received --> leave function
				return len;
		}
	}
	else
	{
		// Split message to parts (packages)
		payloadLen = MSG_PAYLOAD;							// Full payload to start with
		totalLen = 0;
		
//		Serial.print(F("New multi-part message: ")); Serial.println(packages);
		do
		{
			// Calculate payload length. If length exceeds MSG_PAYLOAD length then use MSG_PAYLOAD length.
			if ((len - totalLen) > MSG_PAYLOAD)
			{
				payloadLen = MSG_PAYLOAD;
			}
			else
			{
				payloadLen = len - totalLen;
			}

			// Send command (2: Next package) byte first and then payload
			Serial2.write(2);

			// Send payload
			bytes = Serial2.write(msg, payloadLen);

			if (bytes != payloadLen)
			{
				// Error sending message
				Serial.print(F("Message sending failed (sent / total): ")); Serial.print(bytes); Serial.print(F(" / ")); Serial.println(payloadLen+3);
				return totalLen + payloadLen;
			}

			// Prepare for next package
			msg += payloadLen;									// Advance msg pointer by payload
			totalLen += payloadLen;								// Update total length

			// Wait for response
			switch (waitForResponse(payloadLen + 1, TIMEOUT))	// Length: Command + Payload
			{
				case 255:
					// Timeout
					Serial.println(F("Response to multi-part Payload data did not arrive in time."));
					return 255;
					break;

				case 254:
					// Invalid response
					return 255;
					break;
				
				case 0:
				default:
					// Response fine --> Payload data correctly received --> Send next part
					break;
			}
		} while (totalLen < len);
	}

	return totalLen;
}


// Receive message (single or multi-part)
u8 r_package[MSG_LEN];
FastCRC8 receiveCRC8;

u8 receiveMessage(u8 *buffer, u8 len)
{
	u32 start_ms, current_ms, diff_ms;
	u8 totalLen=0, bytes, response, readLen;
	u8 *bufferStart = buffer;

	// Check if there is data available at Serial port (just to be sure)
	if (Serial2.available())
	{
		// Data available (assumed new message) --> start reading the command
		r_package[0] = Serial2.read();			// Command
		r_package[1] = Serial2.read();			// Payload length
		r_package[2] = Serial2.read();			// CRC8 SMBUS

		// Check header
		if (r_package[0] != 0x01)
		{
			// Invalid command - this is not a new message but trash --> empty it before leaving
			readRemainingMsg();
			Serial.println(F("Invalid new message."));
			return 255;
		}

		// Valid New message command --> Respond
		Serial2.write(3);
		
		// Next is to wait (until timeout) for Payload data
		start_ms = millis();
		while (Serial2.available() == 0) 
		{
			// Wait in a loop until timeout or response
			current_ms = millis();
			diff_ms = current_ms - start_ms;
			if (diff_ms > TIMEOUT)
			{
				// Timeout occurred first
				Serial.println(F("Timeout after New message while waiting for Payload."));
				return 255;
			}
		}

		// Data available --> read up to MSG_PAYLOAD length
		if (r_package[1] <=  MSG_PAYLOAD)
		{
			// Single part message
			// - read command (1 byte) and check it
			response = Serial2.read();
			if (response == 3)
			{
				// Message transmission cancelled (receiver responded incorrect number of bytes to sender)
				Serial.println(F("Message transmission cancelled."));
				readRemainingMsg();
				return 255;
			}

			if (response != 2)
			{
				// Invalid command (no response) --> empty it before leaving
				Serial.print(F("Invalid command received: "));  Serial.println(response);
				readRemainingMsg();
				return 255;
			}

			// Valid command (Next package) received
			bytes = Serial2.readBytes(buffer, r_package[1]);			// Read Payload

			if (bytes != r_package[1])
			{
				// Error reading payload - not enough data to read
				Serial.print(F("Message receiving failed (read / total): ")); Serial.print(bytes); Serial.print(F(" / ")); Serial.println(r_package[1]);
				return 255;
			}
			else
			{
				//  Message received successfully --> Send response
				Serial2.write(r_package[1] + 1);

				// CRC8 check
				if (r_package[2] != receiveCRC8.smbus(buffer, r_package[1]))
				{
					// CRC8 check failed
					// - read assumed length of remaining data
					Serial.print(F("Receive failed. CRC header / calculated: ")); Serial.print(r_package[2], HEX); Serial.print(F(" / ")); Serial.println(receiveCRC8.smbus(buffer, r_package[1]), HEX);
					readRemainingMsg();
					return 255;
				}
				else
				{
					// CRC8 check passed --> All fine and return with number of bytes received

					return r_package[1];
				}
			}
		}
		else
		{
			// Multi-part message
			readLen = MSG_PAYLOAD;						// First part has full Payload

			while (totalLen < r_package[1])
			{
				// Read command and check it
				response = Serial2.read();
				if (response == 3)
				{
					// Message transmission cancelled (receiver responded incorrect number of bytes to sender)
					Serial.println(F("Message transmission cancelled."));
					readRemainingMsg();
					return 255;
				}

				if (response != 2)
				{
					// Invalid command (no response) --> empty it before leaving
					Serial.print(F("Invalid command received: "));  Serial.println(response);
					readRemainingMsg();
					return 255;
				}

				// Valid command (Next package) received
				bytes = Serial2.readBytes(buffer, readLen);			// Read Payload

				if (bytes != readLen)
				{
					// Error reading payload - not enough data to read
					Serial.print(F("Message receiving failed (read / total): ")); Serial.print(bytes); Serial.print(F(" / ")); Serial.println(r_package[1]);
					return 255;
				}

				// Received correct number of bytes --> response (it does not matter if later CRC fails, sender cannot do anything about it)
				Serial2.write(readLen + 1);

				// Prepare for next part
				totalLen += readLen;								// Keep track of received bytes
				buffer   += readLen;								// Advance buffer pointer for next payload

				if ( (r_package[1] - totalLen) >  MSG_PAYLOAD )
				{
					// Next part has full Payload
					readLen = MSG_PAYLOAD;
				}
				else
				{
					// Last part has less than full Payload
					readLen = r_package[1] - totalLen;
					//Serial.print(F("Last package (length): ")); Serial.println(readLen);
				}


				// if there's data still to come: Wait (until timeout) for next Payload data
				if (totalLen <  r_package[1])
				{
					start_ms = millis();
					while (Serial2.available() == 0) 
					{
						// Wait in a loop until timeout or response
						current_ms = millis();
						diff_ms = current_ms - start_ms;
						if (diff_ms > TIMEOUT)
						{
							// Timeout occurred first
							Serial.print(F("Timeout :")); Serial.print(diff_ms);
							Serial.print(F("   Total length: ")); Serial.println(totalLen);
							return 255;
						}
					}
				}
			}

			// All bytes received --> Check CRC (use original buffer start pointer)
			if (r_package[2] != receiveCRC8.smbus(bufferStart, r_package[1]))
			{
				// CRC8 check failed
				// - read assumed length of remaining data
				Serial.print(F("Receive failed. CRC header / calculated: ")); Serial.print(r_package[2]); Serial.print(F(" / ")); Serial.println(receiveCRC8.smbus(buffer, r_package[1]));
				readRemainingMsg();
				return 255;
			}
			else
			{
				// CRC8 check passed --> All fine and return with number of bytes received
				return totalLen;
			}

		}
	}
	else
	{
		// No data available --> leave
		return 0;
	}
}

// Read remaining assumed length of transferred data to buffer - no error checking needed as this is trash
void readRemainingMsg()
{
	u8 i = 0, byte;
	while (Serial2.available())
	{
		byte = Serial2.read();
		Serial.print(byte, HEX); Serial.print(F(" "));
		i++;
	}

	Serial.print(F("Remaining read: ")); Serial.println(i);

	return;
}


// Wait for response and check result or timeout
u8 waitForResponse(u8 expectedData, u8 timeout)
{
	u32 start_ms, current_ms, diff_ms; 
	u8 response;

	start_ms = millis();
	while (Serial2.available() == 0) 
	{
		// Wait in a loop until timeout or response
		current_ms = millis();
		diff_ms = current_ms - start_ms;
		if (diff_ms > timeout)
		{
			// Timeout occurred first
			return 255;
		}
	}

	// Response received --> read and check
	response = Serial2.read();

	if (response != expectedData)
	{
		// Invalid response
		Serial.print(F("Invalid response (received / expected): ")); Serial.print(response); Serial.print(F(" / ")); Serial.println(expectedData);
		return 254;
	}

	return 0;
}