Below is a snippet from ReceiveData(). The issue is the "if (b == R200_FrameEnd) {break;}" line breaks the inner while loop but since it doesn't break the outer while loop. This means that if more than one message is pending (race condition) messages after the first can be read in and the rest of the code is not handling multiple messages returned from ReceiveData(). Easiest solution looks to be modifying ReceiveData() to also exit the outer while loop on a Frame_End.
while ((millis() - startTime) < timeOut) {
while (_serial->available()) {
uint8_t b = _serial->read();
if(bytesReceived > RX_BUFFER_LENGTH - 1) {
Serial.print("Error: Max Buffer Length Exceeded!");
flush();
return false;
}
else {
_buffer[bytesReceived] = b;
}
bytesReceived++;
if (b == R200_FrameEnd) { break; }
}
}