diff options
| -rw-r--r-- | metawatch/metawatch.cpp | 23 |
1 files changed, 19 insertions, 4 deletions
diff --git a/metawatch/metawatch.cpp b/metawatch/metawatch.cpp index c287ccb..66fa171 100644 --- a/metawatch/metawatch.cpp +++ b/metawatch/metawatch.cpp @@ -923,12 +923,27 @@ void MetaWatch::realReceive(bool block) /* Still not enough data available. */ return; /* Wait for more, if non blocking. */ } - char header[4]; + static const int HEADER_SIZE = 4; + char header[HEADER_SIZE]; - dataRead = _socket->read(header, 4); - if (dataRead < 4 || header[0] != 0x01) { - qWarning() << "TODO: Resync to start of frame"; + dataRead = _socket->read(header, HEADER_SIZE); + if (dataRead < 4) { + qWarning() << "Short read"; return; + } else if (header[0] != 0x01) { + qWarning() << "Header not found, trying to recover"; + // Let's try to find the header in one of the four bits we read + for (int i = 1; i < HEADER_SIZE; i++) { + if (header[i] == 0x01) { + // Header possibly found, try to recover by pushing + // the partial header back into the buffer and retrying + for (int j = HEADER_SIZE - 1; j >= i; j--) { + _socket->ungetChar(header[j]); + } + } + } + // In any case, try to repeat. + continue; } _partialReceived.type = static_cast<MessageType>(header[2]); |