попытки сделать crsf, не взлетела часть с пультом

lib/crsf.cpp

@@ -0,0 +1,138 @@
+#include "crsf.h"
+
+// rawBuffer — полный CRSF-пакет:
+// [0] = preamble (0xC8)
+// [1] = size
+// [2] = address
+// [3] = type
+// [4..N-2] = payload
+// [N-1] = crc
+crsf::CrsfFrame::CrsfFrame(std::span<const uint8_t> rawBuffer) {
+    if (rawBuffer.size() < 6) {
+        // минимальный размер CRSF кадра — 6 байт
+        return;
+    }
+
+    uint8_t size = rawBuffer[1];
+    if (rawBuffer.size() != size + 2) {
+        // размер несоответствует фактической длине буфера
+        return;
+    }
+
+    address = rawBuffer[2];
+    type    = rawBuffer[3];
+
+    size_t payloadSize = size - 2; // address + type занимают первые 2
+    payload.assign(rawBuffer.begin() + 4, rawBuffer.begin() + 4 + payloadSize);
+
+    crc = rawBuffer[rawBuffer.size() - 1];
+}
+
+bool crsf::CrsfFrame::checkCrc() const {
+    // CRC считается по address + type + payload
+    uint8_t expected = crsfComputeCrc(std::span<const uint8_t>(payload.empty()
+            ? nullptr : &payload[0], payload.size() + 2));
+
+    // Нам нужно подготовить временный буфер, потому что адрес и тип не в payload
+    std::vector<uint8_t> tmp;
+    tmp.reserve(payload.size() + 2);
+    tmp.push_back(address);
+    tmp.push_back(type);
+    tmp.insert(tmp.end(), payload.begin(), payload.end());
+
+    expected = crsfComputeCrc(tmp);
+
+    return expected == crc;
+}
+void crsf::CrsfFrame::setCrc() {
+    // аналогично checkCrc, но записываем CRC в поле структуры
+    std::vector<uint8_t> tmp;
+    tmp.reserve(payload.size() + 2);
+    tmp.push_back(address);
+    tmp.push_back(type);
+    tmp.insert(tmp.end(), payload.begin(), payload.end());
+
+    crc = crsfComputeCrc(tmp);
+}
+void crsf::CrsfFrame::writeToBuffer(std::vector<uint8_t>& dest) const {
+    dest.clear();
+
+    const uint8_t preamble = 0xC8;
+    const uint8_t size = static_cast<uint8_t>(2 + payload.size() + 1); // addr + type + payload + crc
+
+    dest.reserve(size + 2);
+
+    dest.push_back(preamble);
+    dest.push_back(size);
+    dest.push_back(address);
+    dest.push_back(type);
+
+    dest.insert(dest.end(), payload.begin(), payload.end());
+
+    // финальный байт crc
+    dest.push_back(crc);
+}
+
+
+uint8_t crsf::crsfComputeCrc(std::span<const uint8_t> data) {
+    uint8_t crc = 0;
+    for (const auto i: data) {
+        crc += i;
+    }
+    return crc;
+}
+
+void crsf::CrsfParser::reset() {
+    bufferPos = 0;
+    frameSize = 0;
+}
+
+void crsf::CrsfParser::parseBytes(const std::vector<uint8_t>& data) {
+    for (auto byte : data) {
+        // преамбула
+        if (bufferPos == 0) {
+            if (byte == PREAMBLE) {
+                buffer[0] = byte;
+                bufferPos = 1;
+            }
+            continue;
+        }
+
+        // чтение size
+        if (bufferPos == 1) {
+            buffer[1] = byte;
+            frameSize = byte;
+
+            if (frameSize < 3 || frameSize > (CRSF_MAX_FRAME_SIZE - 2)) {
+                reset();
+                continue;
+            }
+
+            bufferPos = 2;
+            continue;
+        }
+
+        // обработка тела кадра
+        buffer[bufferPos++] = byte;
+
+        const size_t fullFrameSize = frameSize + 2; // preamble + size + frameSize bytes
+
+        if (bufferPos == fullFrameSize) {
+            auto frame = std::make_unique<CrsfFrame>(std::span(buffer, bufferPos));
+            if (frame->checkCrc()) {
+                frames.push_back(std::move(frame));
+            }
+
+            reset();
+        }
+    }
+}
+
+std::unique_ptr<crsf::CrsfFrame> crsf::CrsfParser::pullPacket() {
+    std::unique_ptr<CrsfFrame> out;
+    if (!this->frames.empty()) {
+        out = std::move(this->frames.front());
+        frames.pop_front();
+    }
+    return out;
+}