void BinaryReceived(const void *_data, size_t length) { const uint8_t *data = (const uint8_t *)_data, *end = data + length; do { /* append new data to buffer, as much as fits there */ auto range = binary_buffer.Write(); if (range.IsEmpty()) { /* overflow: reset buffer to recover quickly */ binary_buffer.Clear(); continue; } size_t nbytes = std::min(size_t(range.length), size_t(end - data)); memcpy(range.data, data, nbytes); data += nbytes; binary_buffer.Append(nbytes); while (true) { range = binary_buffer.Read(); if (range.IsEmpty()) break; size_t nbytes = HandleBinary(range.data, range.length); if (nbytes == 0) { if (binary_buffer.IsFull()) binary_buffer.Clear(); break; } binary_buffer.Consume(nbytes); } } while (data < end); }
char *GetLine() { const auto src = buffer.Read(); char *const end = src.data + src.length; /* a NMEA line starts with a dollar symbol ... */ char *dollar = std::find(src.data, end, '$'); if (dollar == end) { buffer.Clear(); return nullptr; } char *start = dollar + 1; /* ... and ends with an asterisk */ char *asterisk = std::find(start, end, '*'); if (asterisk + 3 > end) /* need more data */ return nullptr; /* verify the checksum following the asterisk (two hex digits) */ const uint8_t calculated_checksum = NMEAChecksum(start, asterisk - start); const char checksum_buffer[3] = { asterisk[1], asterisk[2], 0 }; char *endptr; const uint8_t parsed_checksum = strtoul(checksum_buffer, &endptr, 16); if (endptr != checksum_buffer + 2 || parsed_checksum != calculated_checksum) { buffer.Clear(); return nullptr; } buffer.Consume(asterisk + 3 - src.data); *asterisk = 0; return start; }
void Flush() { port.Flush(); buffer.Clear(); }
void PFLAX() { binary = true; binary_buffer.Clear(); }
bool WesterboerVW921Device::DataReceived(const void *_data, size_t length, struct NMEAInfo &info) { assert(_data != NULL); assert(length > 0); bool result = false; const char *data = (const char *)_data, *end = data + length; do { // append new data to buffer, as much as fits there auto range = buffer.Write(); if (range.IsEmpty()) { // overflow: reset buffer to recover quickly buffer.Clear(); continue; } size_t nbytes = std::min(size_t(range.length), size_t(end - data)); memcpy(range.data, data, nbytes); data += nbytes; buffer.Append(nbytes); while (true) { // read data from the buffer, to see if there's a dollar character range = buffer.Read(); if (range.IsEmpty()) break; // Search for the dollar sign (sync byte) char *dollar = (char *)memchr(range.data, '$', range.length); if (dollar == NULL) // no dollar sign here: wait for more data break; // Make sure there are at least 5 bytes in the buffer to // read the sentence header unsigned remaining_length = range.length - (dollar - range.data); if (remaining_length < 5) break; // Check this is a Westerboer sentence if (dollar[1] != 'w') { // Skip this sentence buffer.Consume(dollar - range.data + 1); continue; } // Read the length of the sentence uint8_t sentence_length = (uint8_t)dollar[2]; // Check if the sentence was completely received already if (remaining_length < sentence_length) break; if (!CheckChecksum(dollar, sentence_length)) { // Skip this sentence buffer.Consume(dollar - range.data + 1); continue; } // Read the sentence identification number uint8_t sentence_number = (uint8_t)dollar[4]; // Parse the sentence SentenceReceived(sentence_number, dollar, sentence_length, info); buffer.Consume(dollar - range.data + sentence_length); result = true; } } while (data < end); return result; }