From 83a2af50681694cf86e20befc215562fdb30d865 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 9 Apr 2026 13:00:05 +0200 Subject: [PATCH] msp over crsf - fragmentation handling --- lib/Espfc/src/Connect/Msp.cpp | 16 +- lib/Espfc/src/Connect/Msp.hpp | 8 +- lib/Espfc/src/Connect/MspProcessor.cpp | 2 +- lib/Espfc/src/Device/InputCRSF.cpp | 32 ++-- lib/Espfc/src/Device/InputCRSF.h | 7 +- lib/Espfc/src/Rc/Crsf.cpp | 143 +++++++++----- lib/Espfc/src/Rc/Crsf.h | 22 +-- lib/Espfc/src/Telemetry/TelemetryCRSF.h | 25 ++- test/test_input_crsf/test_input_crsf.cpp | 229 ++++++++++++++++++++++- 9 files changed, 378 insertions(+), 106 deletions(-) diff --git a/lib/Espfc/src/Connect/Msp.cpp b/lib/Espfc/src/Connect/Msp.cpp index ea903751..3ad179ac 100644 --- a/lib/Espfc/src/Connect/Msp.cpp +++ b/lib/Espfc/src/Connect/Msp.cpp @@ -1,12 +1,11 @@ #include "Connect/Msp.hpp" #include "Hal/Pgm.h" #include "Utils/Crc.hpp" +#include -namespace Espfc { +namespace Espfc::Connect { -namespace Connect { - -MspMessage::MspMessage(): state(MSP_STATE_IDLE), expected(0), received(0), read(0) {} +MspMessage::MspMessage(): state(MSP_STATE_IDLE), expected(0), received(0), read(0), sequence(0) {} bool MspMessage::isReady() const { @@ -56,6 +55,13 @@ uint32_t MspMessage::readU32() return ret; } +uint16_t MspMessage::append(const uint8_t * data, size_t len) +{ + std::copy(data, data + len, buffer + received); + received += len; + return received; +} + MspResponse::MspResponse(): len(0) {} int MspResponse::remain() const @@ -170,5 +176,3 @@ size_t MspResponse::serializeV2(uint8_t * buff, size_t len_max) const } } - -} diff --git a/lib/Espfc/src/Connect/Msp.hpp b/lib/Espfc/src/Connect/Msp.hpp index 39ac2633..5ed636b5 100644 --- a/lib/Espfc/src/Connect/Msp.hpp +++ b/lib/Espfc/src/Connect/Msp.hpp @@ -4,9 +4,7 @@ #include #include "Hal/Pgm.h" -namespace Espfc { - -namespace Connect { +namespace Espfc::Connect { constexpr size_t MSP_BUF_SIZE = 192; constexpr size_t MSP_BUF_OUT_SIZE = 240; @@ -61,6 +59,7 @@ class MspMessage uint8_t readU8(); uint16_t readU16(); uint32_t readU32(); + uint16_t append(const uint8_t * data, size_t len); MspState state; MspType dir; @@ -70,6 +69,7 @@ class MspMessage uint16_t expected; uint16_t received; uint16_t read; + uint8_t sequence; uint8_t checksum; uint8_t checksum2; uint8_t buffer[MSP_BUF_SIZE]; @@ -101,5 +101,3 @@ class MspResponse }; } - -} diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp index 35787bfa..086c5cfc 100644 --- a/lib/Espfc/src/Connect/MspProcessor.cpp +++ b/lib/Espfc/src/Connect/MspProcessor.cpp @@ -1629,7 +1629,7 @@ void MspProcessor::sendResponse(MspResponse& r, Device::SerialDevice& s) { debugResponse(r); uint8_t buff[256]; - size_t len = r.serialize(buff, 256); + size_t len = r.serialize(buff, sizeof(buff)); s.write(buff, len); } diff --git a/lib/Espfc/src/Device/InputCRSF.cpp b/lib/Espfc/src/Device/InputCRSF.cpp index 39597397..a58066ed 100644 --- a/lib/Espfc/src/Device/InputCRSF.cpp +++ b/lib/Espfc/src/Device/InputCRSF.cpp @@ -2,9 +2,7 @@ #include "InputCRSF.h" #include "Utils/MemoryHelper.h" -namespace Espfc { - -namespace Device { +namespace Espfc::Device { using namespace Espfc::Rc; @@ -84,7 +82,7 @@ void FAST_CODE_ATTR InputCRSF::parse(CrsfMessage& msg, int d) } break; case CRSF_SIZE: - if(c > 3 && c <= CRSF_PAYLOAD_SIZE_MAX) + if(c >= 2 && c <= CRSF_FRAME_SIZE_MAX - 2) // allowed size is in range 2-62 { data[_idx++] = c; _state = CRSF_TYPE; @@ -93,10 +91,14 @@ void FAST_CODE_ATTR InputCRSF::parse(CrsfMessage& msg, int d) } break; case CRSF_TYPE: - if(c == CRSF_FRAMETYPE_RC_CHANNELS_PACKED || c == CRSF_FRAMETYPE_LINK_STATISTICS || c == CRSF_FRAMETYPE_MSP_REQ) + if(c == CRSF_FRAMETYPE_RC_CHANNELS_PACKED || c == CRSF_FRAMETYPE_LINK_STATISTICS || c == CRSF_FRAMETYPE_MSP_REQ || c == CRSF_FRAMETYPE_MSP_WRITE) { data[_idx++] = c; - _state = CRSF_DATA; + if (msg.size > 2) { + _state = CRSF_DATA; + } else { + _state = CRSF_CRC; // no payload, next byte is crc + } } else { reset(); } @@ -138,6 +140,7 @@ void FAST_CODE_ATTR InputCRSF::apply(const CrsfMessage& msg) break; case CRSF_FRAMETYPE_MSP_REQ: + case CRSF_FRAMETYPE_MSP_WRITE: applyMspReq(msg); break; @@ -148,31 +151,30 @@ void FAST_CODE_ATTR InputCRSF::apply(const CrsfMessage& msg) void FAST_CODE_ATTR InputCRSF::applyLinkStats(const CrsfMessage& msg) { - const CrsfLinkStats* stats = reinterpret_cast(msg.payload); + const auto * stats = reinterpret_cast(msg.payload); (void)stats; // TODO: } void FAST_CODE_ATTR InputCRSF::applyChannels(const CrsfMessage& msg) { - const CrsfData* data = reinterpret_cast(msg.payload); + const auto * data = reinterpret_cast(msg.payload); Crsf::decodeRcDataShift8(_channels, data); //Crsf::decodeRcData(_channels, frame); _new_data = true; } -void FAST_CODE_ATTR InputCRSF::applyMspReq(const CrsfMessage& msg) +void FAST_CODE_ATTR InputCRSF::applyMspReq(const CrsfMessage& frame) { if(!_telemetry) return; - uint8_t origin; - Connect::MspMessage m; + uint8_t origin = 0; - Crsf::decodeMsp(msg, m, origin); + Crsf::decodeMsp(frame, _msg, origin); - if(m.isCmd() && m.isReady()) + if(_msg.isCmd() && _msg.isReady()) { - _telemetry->processMsp(*_serial, TELEMETRY_PROTOCOL_CRSF, m, origin); + _telemetry->processMsp(*_serial, TELEMETRY_PROTOCOL_CRSF, _msg, origin); } _telemetry_next = micros() + TELEMETRY_INTERVAL; @@ -180,5 +182,3 @@ void FAST_CODE_ATTR InputCRSF::applyMspReq(const CrsfMessage& msg) } -} - diff --git a/lib/Espfc/src/Device/InputCRSF.h b/lib/Espfc/src/Device/InputCRSF.h index 465fd9aa..e05c59bf 100644 --- a/lib/Espfc/src/Device/InputCRSF.h +++ b/lib/Espfc/src/Device/InputCRSF.h @@ -9,9 +9,7 @@ // https://github.com/AlessioMorale/crsf_parser/tree/master // https://github.com/betaflight/betaflight/blob/master/src/main/rx/crsf.c -namespace Espfc { - -namespace Device { +namespace Espfc::Device { class InputCRSF: public InputDevice { @@ -54,8 +52,7 @@ class InputCRSF: public InputDevice Rc::CrsfMessage _frame; uint16_t _channels[CHANNELS]; uint32_t _telemetry_next; + Connect::MspMessage _msg; }; } - -} diff --git a/lib/Espfc/src/Rc/Crsf.cpp b/lib/Espfc/src/Rc/Crsf.cpp index 147cc267..158ca6dd 100644 --- a/lib/Espfc/src/Rc/Crsf.cpp +++ b/lib/Espfc/src/Rc/Crsf.cpp @@ -5,9 +5,7 @@ #include "Utils/MemoryHelper.h" #include -namespace Espfc { - -namespace Rc { +namespace Espfc::Rc { void FAST_CODE_ATTR Crsf::decodeRcData(uint16_t* channels, const CrsfData* frame) { @@ -92,77 +90,128 @@ void Crsf::encodeRcData(CrsfMessage& msg, const CrsfData& data) msg.payload[sizeof(data)] = crc(msg); } -int Crsf::encodeMsp(CrsfMessage& msg, const Connect::MspResponse& resp, uint8_t origin) +const uint8_t* Crsf::encodeMspData(CrsfMessage& msg, uint8_t origin, uint8_t version, uint8_t seq, bool start, const uint8_t* begin, const uint8_t* end) { - uint8_t buff[CRSF_PAYLOAD_SIZE_MAX]; - size_t size = resp.serialize(buff, CRSF_PAYLOAD_SIZE_MAX); - - if(size < 4) return 0; // unable to serialize + auto fragmentEnd = std::min(begin + CRSF_PAYLOAD_SIZE_MAX - 1, end); // preserve space for crc uint8_t status = 0; - status |= (1 << 4); // start bit - status |= ((resp.version == Connect::MSP_V1 ? 1 : 2) << 5); + status |= (seq & CRSF_MSP_STATUS_SEQ_MASK); // sequence number + status |= start ? (1 << 4) : 0; // start bit + status |= ((version << 5) & CRSF_MSP_STATUS_VERSION_MASK); // msp version (1 or 2) msg.prepare(Rc::CRSF_FRAMETYPE_MSP_RESP); msg.writeU8(origin); msg.writeU8(Rc::CRSF_ADDRESS_FLIGHT_CONTROLLER); msg.writeU8(status); - msg.write(buff + 3, size - 4); // skip sync bytes and crc + msg.write(begin, fragmentEnd - begin); msg.finalize(); - return msg.size; + return fragmentEnd; +} + +template +static inline void fillMessage(const CrsfMessage& frame, Connect::MspMessage& m, Connect::MspVersion version) +{ + // Payload structure: [dst, origin, flags, msp_header, msp_data...] + // Available MSP data (after header) = frame.size - 5 (type,dst,origin,flags,crc) - sizeof(header) + const auto * hdr = reinterpret_cast(frame.payload + 3); + const size_t framePayloadSize = frame.size - 5 - sizeof(HeaderType); + m.cmd = hdr->cmd; + m.version = version; + m.dir = Connect::MSP_TYPE_CMD; + m.expected = hdr->size; + m.append(frame.payload + 3 + sizeof(HeaderType), std::min(framePayloadSize, (size_t)hdr->size)); // skip dst, origin, status and msp header + if(m.expected == m.received) + { + m.state = Connect::MSP_STATE_RECEIVED; + } } -int Crsf::decodeMsp(const CrsfMessage& msg, Connect::MspMessage& m, uint8_t& origin) +int FAST_CODE_ATTR Crsf::decodeMsp(const CrsfMessage& frame, Connect::MspMessage& m, uint8_t& origin) { + // 0x7A, 0x7C + // CRSF frame which wraps MSP request (‘$M<’ or ‘$X<’) + // Supported by Betaflight devices + // Supported devices will respond with 0x7B + + // 0x7B + // CRSF frame which wraps MSP response (‘$M>’,’$X>’,‘$M!’,’$X!’) + // Supported by Betaflight devices + // Supported device will send this frame in response of MSP_Request (0x7A) + + // MSP frame over CRSF Payload packing: + // MSP frame is stripped from header ($ + M/X + [/]/!) and CRC + // Resulted MSP-body might be divided in chunks if it doesn't fit in one CRSF-frame. + // A ‘Status’ byte is put before MSP-body in each CRSF-frame. + // Status byte consists of three parts: + // bits 0-3 represent cyclic sequence number of the CRSF frame; + // bit 4 checks if current MSP chunk is the beginning (or only) of a new frame (1 if true); + // bits 5-6 represent the version number of MSP protocol (1 or 2 currently); + // bit 7 represents an error (for response only). + // Chunk size of the MSP-body is calculated from size of CRSF frame. But size of the MSP-body + // must be parsed from the MSP-body itself (with respect to MSP version and Jumbo-frame). + // The last/only CRSF-frame might be longer than needed. In such a case, the extra bytes must be ignored. + // Maximum chunk size is defined by maximum length of CRSF frame 64 bytes, therefore, maximum MSP-chunk length is 57 bytes. + // Minimum chunk length might by anything, but the first chunk must consist of size and function ID (i.e., 5 bytes for MSPv2). + // CRC of the MSP frame is not sent because it’s already protected by CRC of CRSF. If MSP CRC is needed, + // it should be calculated at the receiving point. + // MSP-response must be sent to the origin of the MSP-request. It means that [destination] and [origin] bytes of CRSF-header + // in response must be the same as in request but swapped. + // see https://github.com/betaflight/betaflight/blob/master/src/main/telemetry/msp_shared.c#L175 + // https://github.com/betaflight/betaflight/blob/538564bbe3eb226d5eac2e3387401bca7c6fdb90/src/main/telemetry/msp_shared.c#L175 + + // frame: [{msp}] //uint8_t dst = msg.payload[0]; - origin = msg.payload[1]; - uint8_t status = msg.payload[2]; + origin = frame.payload[1]; + uint8_t status = frame.payload[2]; - //uint8_t sequence = (status & 0x0f); // 00001111 - uint8_t start = (status & 0x10) >> 4; // 00010000 - uint8_t version = (status & 0x60) >> 5; // 01100000 - //uint8_t error = (status & 0x80) >> 7; // 10000000 + uint8_t sequence = (status & CRSF_MSP_STATUS_SEQ_MASK); // 00001111 + uint8_t start = (status & CRSF_MSP_STATUS_START_MASK) >> 4; // 00010000 + uint8_t version = (status & CRSF_MSP_STATUS_VERSION_MASK) >> 5; // 01100000 + //uint8_t error = (status & CRSF_MSP_STATUS_ERROR_MASK) >> 7; // 10000000 if(start) { + // reset message on start + m.state = Connect::MSP_STATE_IDLE; + m.received = 0; + m.expected = 0; if(version == 1) { - const Connect::MspHeaderV1 * hdr = reinterpret_cast(msg.payload + 3); - size_t framePayloadSize = msg.size - 5 - sizeof(Connect::MspHeaderV1); - if(framePayloadSize >= hdr->size) - { - m.expected = hdr->size; - m.received = hdr->size; - m.cmd = hdr->cmd; - m.state = Connect::MSP_STATE_RECEIVED; - m.dir = Connect::MSP_TYPE_CMD; - m.version = Connect::MSP_V1; - std::copy_n(msg.payload + 3 + sizeof(Connect::MspHeaderV1), m.received, m.buffer); - } + fillMessage(frame, m, Connect::MSP_V1); } else if(version == 2) { - const Connect::MspHeaderV2 * hdr = reinterpret_cast(msg.payload + 3); - size_t framePayloadSize = msg.size - 5 - sizeof(Connect::MspHeaderV2); - if(framePayloadSize >= hdr->size) - { - m.expected = hdr->size; - m.received = hdr->size; - m.cmd = hdr->cmd; - m.state = Connect::MSP_STATE_RECEIVED; - m.dir = Connect::MSP_TYPE_CMD; - m.version = Connect::MSP_V1; - std::copy_n(msg.payload + 3 + sizeof(Connect::MspHeaderV2), m.received, m.buffer); - } + fillMessage(frame, m, Connect::MSP_V2); } } else { - // next chunk + // next chunks - continuation of fragmented message + if(sequence == ((m.sequence + 1) & CRSF_MSP_STATUS_SEQ_MASK)) + { + size_t framePayloadSize = std::min(frame.size - 5, m.expected - m.received); // skip dst, origin, status; + if(m.received + framePayloadSize <= Connect::MSP_BUF_SIZE) + { + m.append(frame.payload + 3, framePayloadSize); + if(m.received == m.expected) + { + m.state = Connect::MSP_STATE_RECEIVED; + } + } + } + else + { + // sequence mismatch - reset message + m.state = Connect::MSP_STATE_IDLE; + m.received = 0; + m.expected = 0; + } } - return 0; + m.sequence = sequence; + + return m.state == Connect::MSP_STATE_RECEIVED ? 1 : 0; } uint16_t Crsf::convert(int v) @@ -187,11 +236,9 @@ uint8_t Crsf::crc(const CrsfMessage& msg) // CRC includes type and payload uint8_t crc = Utils::crc8_dvb_s2(0, msg.type); for (int i = 0; i < msg.size - 2; i++) { // size includes type and crc - crc = Utils::crc8_dvb_s2(crc, msg.payload[i]); + crc = Utils::crc8_dvb_s2(crc, msg.payload[i]); } return crc; } } - -} diff --git a/lib/Espfc/src/Rc/Crsf.h b/lib/Espfc/src/Rc/Crsf.h index 77eb87d8..ed5804ff 100644 --- a/lib/Espfc/src/Rc/Crsf.h +++ b/lib/Espfc/src/Rc/Crsf.h @@ -5,9 +5,7 @@ #include "Utils/Crc.hpp" #include "Connect/Msp.hpp" -namespace Espfc { - -namespace Rc { +namespace Espfc::Rc { enum { CRSF_SYNC_BYTE = 0xC8 }; enum { CRSF_FRAME_SIZE_MAX = 64 }; // 62 bytes frame plus 2 bytes frame header() @@ -119,24 +117,23 @@ struct CrsfMessage uint8_t addr; // CrsfAddress uint8_t size; // counts size after this byte, so it must be the payload size + 2 (type and crc) uint8_t type; // CrsfFrameType - uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; + uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; // +1 for crc void prepare(uint8_t t) { addr = Rc::CRSF_SYNC_BYTE; type = t; - size = 0; + size = 2; } void finalize() { - size += 2; writeCRC(crc()); } void writeU8(uint8_t v) { - payload[size++] = v; + payload[size++ - 2] = v; } void writeU16(uint16_t v) @@ -183,12 +180,15 @@ class Crsf static void decodeRcDataShift8(uint16_t* channels, const CrsfData* frame); //static void decodeRcDataShift32(uint16_t* channels, const CrsfData* frame); static void encodeRcData(CrsfMessage& frame, const CrsfData& data); - static int encodeMsp(CrsfMessage& msg, const Connect::MspResponse& res, uint8_t origin); - static int decodeMsp(const CrsfMessage& msg, Connect::MspMessage& m, uint8_t& origin); + static const uint8_t* encodeMspData(CrsfMessage& msg, uint8_t origin, uint8_t version, uint8_t seq, bool start, const uint8_t* begin, const uint8_t* end); + static int decodeMsp(const CrsfMessage& frame, Connect::MspMessage& m, uint8_t& origin); static uint16_t convert(int v); static uint8_t crc(const CrsfMessage& frame); -}; -} + static constexpr uint8_t CRSF_MSP_STATUS_SEQ_MASK = 0x0F; + static constexpr uint8_t CRSF_MSP_STATUS_START_MASK = 0x10; + static constexpr uint8_t CRSF_MSP_STATUS_VERSION_MASK = 0x60; + static constexpr uint8_t CRSF_MSP_STATUS_ERROR_MASK = 0x80; +}; } diff --git a/lib/Espfc/src/Telemetry/TelemetryCRSF.h b/lib/Espfc/src/Telemetry/TelemetryCRSF.h index d98ab290..b0d109f2 100644 --- a/lib/Espfc/src/Telemetry/TelemetryCRSF.h +++ b/lib/Espfc/src/Telemetry/TelemetryCRSF.h @@ -73,15 +73,22 @@ class TelemetryCRSF return 1; } - int sendMsp(Device::SerialDevice& s, Connect::MspResponse r, uint8_t origin) const + int sendMsp(Device::SerialDevice& s, Connect::MspResponse resp, uint8_t origin) { - Rc::CrsfMessage msg; - - Rc::Crsf::encodeMsp(msg, r, origin); - - send(msg, s); + size_t size = resp.serialize(_buff, sizeof(_buff)); + const uint8_t* beg = _buff + 3; // skip msp header + const uint8_t* end = _buff + size - 1; // skip crc + uint8_t version = resp.version == Connect::MSP_V1 ? 1 : 2; + size_t iter = 0; + Rc::CrsfMessage frame; + do + { + beg = Rc::Crsf::encodeMspData(frame, origin, version, _seq++, !iter, beg, end); + send(frame, s); + iter++; + } while(beg != end && iter < 4); - return 1; + return iter; } void send(const Rc::CrsfMessage& msg, Device::SerialDevice& s) const @@ -93,7 +100,7 @@ class TelemetryCRSF { if(angle < -Utils::pi()) angle += Utils::twoPi(); if(angle > Utils::pi()) angle -= Utils::twoPi(); - return lrintf(angle * 1000); + return lrintf(angle * 10000); } void attitude(Rc::CrsfMessage& msg) const @@ -182,6 +189,8 @@ class TelemetryCRSF private: Model& _model; mutable TelemetryState _current; + uint8_t _buff[256] = {0}; + uint8_t _seq = 0; }; } diff --git a/test/test_input_crsf/test_input_crsf.cpp b/test/test_input_crsf/test_input_crsf.cpp index 3bcf8980..4d46f71b 100644 --- a/test/test_input_crsf/test_input_crsf.cpp +++ b/test/test_input_crsf/test_input_crsf.cpp @@ -49,6 +49,37 @@ void test_input_crsf_rc_valid() TEST_ASSERT_EQUAL_UINT16(1000u, input.get(5)); } +void test_input_crsf_rc_valid_no_payload() +{ + InputCRSF input; + CrsfMessage frame; + memset(&frame, 0, sizeof(frame)); + uint8_t * frame_data = reinterpret_cast(&frame); + + When(Method(ArduinoFake(), micros)).Return(0); + + input.begin(nullptr, nullptr); + + const uint8_t data[] = { + 0xC8, 0x02, 0x16, 0xD3 + }; + for (size_t i = 0; i < sizeof(data); i++) { + input.parse(frame, data[i]); + } + + for (size_t i = 0; i < sizeof(data); i++) { + TEST_ASSERT_EQUAL_UINT8(data[i], frame_data[i]); + } + + const uint8_t crc = Crsf::crc(frame); + TEST_ASSERT_EQUAL_UINT8(0xD3, crc); + TEST_ASSERT_EQUAL_UINT8(0xD3, frame.crc()); + + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(0x02, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.type); +} + void test_input_crsf_rc_prefix() { InputCRSF input; @@ -297,6 +328,25 @@ void test_crsf_decode_rc_shift8() TEST_ASSERT_EQUAL_UINT16(1500, channels[15]); }*/ +void test_crsf_encode_tlm() +{ + CrsfMessage frame; + memset(&frame, 0, sizeof(frame)); + + frame.prepare(CRSF_FRAMETYPE_HEARTBEAT); + frame.writeU8(0x01); + frame.finalize(); + + TEST_ASSERT_EQUAL_UINT8(0xC8, frame.addr); // addr + TEST_ASSERT_EQUAL_UINT8(0x03, frame.size); // size + TEST_ASSERT_EQUAL_UINT8(0x0B, frame.type); // type: heartbeat + TEST_ASSERT_EQUAL_UINT8(0x01, frame.payload[0]); // payload + TEST_ASSERT_EQUAL_UINT8(0x90, frame.payload[1]); // crc + + const uint8_t crc = Crsf::crc(frame); + TEST_ASSERT_EQUAL_UINT8(0x90, crc); +} + void test_crsf_encode_msp_v1() { CrsfMessage frame; @@ -310,7 +360,14 @@ void test_crsf_encode_msp_v1() resp.writeU8(2); resp.writeU8(3); - Crsf::encodeMsp(frame, resp, CRSF_ADDRESS_RADIO_TRANSMITTER); + uint8_t buff[255]; + size_t size = resp.serialize(buff, sizeof(buff)); + const uint8_t* beg = buff + 3; // skip msp header + const uint8_t* end = buff + size - 1; // skip crc + uint8_t seq = 0; + + beg = Crsf::encodeMspData(frame, CRSF_ADDRESS_RADIO_TRANSMITTER, 1, seq++, true, beg, end); + TEST_ASSERT_TRUE(beg == end); // crsf headers TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); @@ -335,6 +392,73 @@ void test_crsf_encode_msp_v1() TEST_ASSERT_EQUAL_UINT8(0x6D, frame.crc()); } +void test_crsf_encode_msp_v1_fragmented() +{ + CrsfMessage frame; + memset(&frame, 0, sizeof(frame)); + + Connect::MspResponse resp; + resp.version = Connect::MSP_V1; + resp.cmd = MSP_API_VERSION; + resp.result = 0; + for(size_t i = 0; i < 64; i++) + { + resp.writeU8(i); + } + + uint8_t buff[255]; + size_t size = resp.serialize(buff, sizeof(buff)); + const uint8_t* beg = buff + 3; // skip msp header + const uint8_t* end = buff + size - 1; // skip crc + uint8_t seq = 0; + + beg = Crsf::encodeMspData(frame, CRSF_ADDRESS_RADIO_TRANSMITTER, 1, seq++, true, beg, end); + TEST_ASSERT_FALSE(beg == end); + + // crsf headers + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(62, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_MSP_RESP, frame.type); + + // crsf ext headers + TEST_ASSERT_EQUAL_UINT8(0xEA, frame.payload[0]); // radio-transmitter + TEST_ASSERT_EQUAL_UINT8(0xC8, frame.payload[1]); // FC + TEST_ASSERT_EQUAL_UINT8(0x30, frame.payload[2]); // status + + // ext msp v1 header + TEST_ASSERT_EQUAL_UINT8(64, frame.payload[3]); // size + TEST_ASSERT_EQUAL_UINT8( 1, frame.payload[4]); // type // api_version(1) + + // ext msp payload + TEST_ASSERT_EQUAL_UINT8(0, frame.payload[5]); // param0 + TEST_ASSERT_EQUAL_UINT8(1, frame.payload[6]); // param1 + TEST_ASSERT_EQUAL_UINT8(2, frame.payload[7]); // param2 + + // crsf crc + TEST_ASSERT_EQUAL_UINT8(0xFE, frame.crc()); + + beg = Crsf::encodeMspData(frame, CRSF_ADDRESS_RADIO_TRANSMITTER, 1, seq++, false, beg, end); + TEST_ASSERT_TRUE(beg == end); + + // crsf headers + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(14, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_MSP_RESP, frame.type); + + // crsf ext headers + TEST_ASSERT_EQUAL_UINT8(0xEA, frame.payload[0]); // radio-transmitter + TEST_ASSERT_EQUAL_UINT8(0xC8, frame.payload[1]); // FC + TEST_ASSERT_EQUAL_UINT8(0x21, frame.payload[2]); // status + + // ext msp payload + TEST_ASSERT_EQUAL_UINT8(55, frame.payload[3]); // param55 + TEST_ASSERT_EQUAL_UINT8(56, frame.payload[4]); // param56 + TEST_ASSERT_EQUAL_UINT8(57, frame.payload[5]); // param57 + + // crsf crc + TEST_ASSERT_EQUAL_UINT8(0x73, frame.crc()); +} + void test_crsf_encode_msp_v2() { CrsfMessage frame; @@ -348,7 +472,14 @@ void test_crsf_encode_msp_v2() resp.writeU8(2); resp.writeU8(3); - Crsf::encodeMsp(frame, resp, CRSF_ADDRESS_RADIO_TRANSMITTER); + uint8_t buff[255]; + size_t size = resp.serialize(buff, sizeof(buff)); + const uint8_t* beg = buff + 3; // skip msp header + const uint8_t* end = buff + size - 1; // skip crc + uint8_t seq = 0xff; + + beg = Crsf::encodeMspData(frame, CRSF_ADDRESS_RADIO_TRANSMITTER, 2, seq, true, beg, end); + TEST_ASSERT_TRUE(beg == end); // crsf headers TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); @@ -358,7 +489,7 @@ void test_crsf_encode_msp_v2() // crsf ext headers TEST_ASSERT_EQUAL_UINT8(0xEA, frame.payload[0]); // radio-transmitter addr TEST_ASSERT_EQUAL_UINT8(0xC8, frame.payload[1]); // FC addr - TEST_ASSERT_EQUAL_UINT8(0x50, frame.payload[2]); // status flags + TEST_ASSERT_EQUAL_UINT8(0x5F, frame.payload[2]); // status flags // ext msp v2 header TEST_ASSERT_EQUAL_UINT8(0, frame.payload[3]); // flags @@ -373,12 +504,12 @@ void test_crsf_encode_msp_v2() TEST_ASSERT_EQUAL_UINT8(3, frame.payload[10]); // param3 // crsf crc - TEST_ASSERT_EQUAL_UINT8(0xF8, frame.crc()); + TEST_ASSERT_EQUAL_UINT8(0x10, frame.crc()); } void test_crsf_decode_msp_v1() { - const uint8_t data[] = { + const uint8_t data[] = { 0xc8, 0x08, 0x7a, 0xc8, 0xea, 0x32, 0x00, 0x70, 0x70, 0x4b }; CrsfMessage frame; @@ -399,10 +530,16 @@ void test_crsf_decode_msp_v1() TEST_ASSERT_EQUAL_UINT8(0xEA, frame.payload[1]); // radio-transmitter addr (origin) TEST_ASSERT_EQUAL_UINT8(0x32, frame.payload[2]); // status flags - // ext msp v2 header + // ext msp v1 header TEST_ASSERT_EQUAL_UINT8(0x00, frame.payload[3]); // size TEST_ASSERT_EQUAL_UINT8(0x70, frame.payload[4]); // type: msp_pid(0x70) + // ext msp payload + TEST_ASSERT_TRUE(m.isReady()); + TEST_ASSERT_EQUAL_UINT8(Connect::MSP_V1, m.version); + TEST_ASSERT_EQUAL_UINT8(0, m.received); + TEST_ASSERT_EQUAL_UINT8(0x70, m.cmd); + // crsf crc TEST_ASSERT_EQUAL_UINT8(0x4B, frame.crc()); @@ -410,6 +547,82 @@ void test_crsf_decode_msp_v1() TEST_ASSERT_EQUAL_UINT8(0xEA, origin); } +void test_csrf_decode_msp_v1_fragmented() +{ + // generate example msp message into buffer + uint8_t buff[64] = {0}; + Connect::MspResponse resp; + resp.version = Connect::MSP_V1; + resp.cmd = MSP_API_VERSION; + resp.result = 0; + for (uint8_t i = 0; i < 32; i++) + { + resp.writeU8(i); + } + resp.serialize(buff, sizeof(buff)); + + // we need only range from 3 to 37 + const size_t dataLen = 34; + const uint8_t *dataPtr = buff + 3; // skip msp header ($M<) + const uint8_t flags1 = (1 << 5) | (1 << 4) | 0x00; // v(1) + start(1) + sequence(0) + const uint8_t flags2 = (1 << 5) | (0 << 4) | 0x01; // v(1) + start(0) + sequence(1) + const uint8_t dst = CRSF_ADDRESS_FLIGHT_CONTROLLER; + const uint8_t src = CRSF_ADDRESS_RADIO_TRANSMITTER; + + // create first fragmented frame + CrsfMessage frame1; + frame1.prepare(CRSF_FRAMETYPE_MSP_REQ); + frame1.writeU8(dst); + frame1.writeU8(src); + frame1.writeU8(flags1); + for (size_t i = 0; i < 17; i++) + { + frame1.writeU8(dataPtr[i]); + } + frame1.finalize(); + TEST_ASSERT_EQUAL_UINT8(22, frame1.size); + + // create second fragmented frame + CrsfMessage frame2; + frame2.prepare(CRSF_FRAMETYPE_MSP_REQ); + frame2.writeU8(dst); + frame2.writeU8(src); + frame2.writeU8(flags2); + for (size_t i = 17; i < dataLen; i++) + { + frame2.writeU8(dataPtr[i]); + } + frame2.finalize(); + TEST_ASSERT_EQUAL_UINT8(22, frame2.size); + + // decode first frame + Connect::MspMessage m; + uint8_t origin = 0; + + Crsf::decodeMsp(frame1, m, origin); + + TEST_ASSERT_FALSE(m.isReady()); // not ready until we get the rest of the data + TEST_ASSERT_TRUE(m.isCmd()); + + // decode second frame + Crsf::decodeMsp(frame2, m, origin); + + // assert that we receied same message that is valid and complete + TEST_ASSERT_TRUE(m.isReady()); + TEST_ASSERT_TRUE(m.isCmd()); + TEST_ASSERT_EQUAL_UINT8(Connect::MSP_V1, m.version); + TEST_ASSERT_EQUAL_UINT8(MSP_API_VERSION, m.cmd); + TEST_ASSERT_EQUAL_UINT8(32, m.received); + TEST_ASSERT_EQUAL_UINT8(32, m.expected); + + for (uint8_t i = 0; i < 32; i++) + { + TEST_ASSERT_EQUAL_UINT8(i, m.buffer[i]); + } + + TEST_ASSERT_EQUAL_UINT8(src, origin); +} + void test_input_ibus_rc_valid() { InputIBUS input; @@ -484,14 +697,18 @@ int main(int argc, char **argv) { UNITY_BEGIN(); RUN_TEST(test_input_crsf_rc_valid); + RUN_TEST(test_input_crsf_rc_valid_no_payload); RUN_TEST(test_input_crsf_rc_prefix); RUN_TEST(test_crsf_encode_rc); RUN_TEST(test_crsf_decode_rc_struct); RUN_TEST(test_crsf_decode_rc_shift8); //RUN_TEST(test_crsf_decode_rc_shift32); + RUN_TEST(test_crsf_encode_tlm); RUN_TEST(test_crsf_encode_msp_v1); + RUN_TEST(test_crsf_encode_msp_v1_fragmented); RUN_TEST(test_crsf_encode_msp_v2); RUN_TEST(test_crsf_decode_msp_v1); + RUN_TEST(test_csrf_decode_msp_v1_fragmented); RUN_TEST(test_input_ibus_rc_valid); return UNITY_END();