138 lines
3.8 KiB
C++
138 lines
3.8 KiB
C++
#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;
|
|
} |