diff --git a/inc/msp/Client.hpp b/inc/msp/Client.hpp index 0d8bcd7..44a2b76 100644 --- a/inc/msp/Client.hpp +++ b/inc/msp/Client.hpp @@ -309,6 +309,14 @@ class Client { */ uint8_t crcV1(const uint8_t id, const ByteVector& data) const; + /** + * @brief crcV1_2 Computes a checksum for MSPv1 messages + * @param start_val uint8_t start value for crc + * @param data Bytes which are part of the checksum + * @return uint8_t checksum + */ + uint8_t crcV1_2(const uint8_t start_val, const ByteVector& data) const; + /** * @brief packMessageV2 Packs data ID and data payload into a MSPv2 * formatted buffer ready for sending to the serial device diff --git a/src/Client.cpp b/src/Client.cpp index 04bd8e1..6b5063c 100644 --- a/src/Client.cpp +++ b/src/Client.cpp @@ -243,6 +243,14 @@ uint8_t Client::crcV1(const uint8_t id, const ByteVector& data) const { return crc; } +uint8_t Client::crcV1_2(const uint8_t start_val, const ByteVector& data) const { + uint8_t crc = start_val; + for(const uint8_t d : data) { + crc = crc ^ d; + } + return crc; +} + ByteVector Client::packMessageV2(const msp::ID id, const ByteVector& data) const { ByteVector msg; @@ -363,12 +371,23 @@ std::pair Client::messageReady(iterator begin, // not even enough data for a header if(available < 6) return std::make_pair(begin, false); - const uint8_t payload_size = uint8_t(*(i + 3)); + uint16_t payload_size = uint8_t(*(i + 3)); + uint8_t extra_idx = 0; + if (payload_size == 0xFF) { + if(available < 8) return std::make_pair(begin, false); + + payload_size = *(i + 5); + payload_size += static_cast(*(i + 6)) << 8; + extra_idx = 2; + if(log_level_ >= DEBUG) std::cout << "received jumbo message with payload_size " + << payload_size << " available bytes " << available <= DEBUG) std::cout << "received jumbo message with length " << len << std::endl; + } + if(log_level_ >= WARNING && !ok_id) { std::cerr << "Message v1 with ID " << size_t(ret.id) << " is not recognised!" << std::endl; @@ -421,7 +451,7 @@ ReceivedMessage Client::processOneMessageV1() { // CRC const uint8_t rcv_crc = extractChar(); - const uint8_t exp_crc = crcV1(id, ret.payload); + const uint8_t exp_crc = crcV1_2(crc_start_val, ret.payload); const bool ok_crc = (rcv_crc == exp_crc); if(log_level_ >= WARNING && !ok_crc) {