diff --git a/CMakeLists.txt b/CMakeLists.txt index 18d9422..c4368f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,8 +6,33 @@ set(CMAKE_CXX_STANDARD 20) option(ENABLE_GROUND_BUILD "Enable build ground station" OFF) option(ENABLE_AIR_BUILD "Enable build air" ON) +if(WIN32) + set(PORT_SPECIFIC_FILES + lib/port/win/poller.cpp + lib/port/win/uart.cpp + lib/port/win/udp.cpp + ) +else () + set(PORT_SPECIFIC_FILES + lib/port/unix/poller.cpp + lib/port/unix/uart.cpp + lib/port/unix/udp.cpp + ) +endif() + +set(LIB_FILES + ${PORT_SPECIFIC_FILES} + lib/crsf.cpp + lib/crsf.h + lib/port/poller-general.cpp + lib/port/poller.h + lib/port/uart.h + lib/port/udp.h +) + if (ENABLE_GROUND_BUILD) message("Enabled ground build!") + # Настройки для Windows if(WIN32) add_definitions(-DWIN32_LEAN_AND_MEAN) @@ -17,18 +42,13 @@ if (ENABLE_GROUND_BUILD) endif() endif() - find_package(SDL2 REQUIRED) - include_directories(${SDL2_INCLUDE_DIRS}) - add_executable(${PROJECT_NAME}-ground - ground/joystick-reader.cpp - ground/joystick-reader.h ground/main.cpp - ground/udp-driver.cpp - ground/udp-driver.h + ${LIB_FILES} ) - target_link_libraries(${PROJECT_NAME}-ground ${SDL2_LIBRARIES} ${EXTRA_LIBS}) + target_link_libraries(${PROJECT_NAME}-ground ${EXTRA_LIBS}) + target_include_directories(${PROJECT_NAME}-ground PRIVATE ground/ lib/) endif() if (ENABLE_AIR_BUILD) @@ -36,6 +56,8 @@ if (ENABLE_AIR_BUILD) add_executable(${PROJECT_NAME}-air air/main.cpp + ${LIB_FILES} ) + target_include_directories(${PROJECT_NAME}-air PRIVATE air/ lib/) endif() diff --git a/air/main.cpp b/air/main.cpp index f2aead8..97999b5 100644 --- a/air/main.cpp +++ b/air/main.cpp @@ -1,229 +1,13 @@ -#include -#include -#include -#include -#include -#include -#include - #include #include -#include #include -#include +#include +#include "port/uart.h" - - -class UDPServer { -private: - int sockfd; - sockaddr_in server_addr, client_addr; - socklen_t client_len; - -public: - UDPServer(uint16_t port) : client_len(sizeof(client_addr)) { - // Создание UDP сокета - sockfd = socket(AF_INET, SOCK_DGRAM, 0); - if (sockfd < 0) { - throw std::runtime_error("Failed to create socket"); - } - - // Настройка адреса сервера - memset(&server_addr, 0, sizeof(server_addr)); - server_addr.sin_family = AF_INET; - server_addr.sin_addr.s_addr = INADDR_ANY; - server_addr.sin_port = htons(port); - - // Привязка сокета - if (bind(sockfd, (sockaddr*)&server_addr, sizeof(server_addr)) < 0) { - close(sockfd); - throw std::runtime_error("Failed to bind socket"); - } - - std::cout << "UDP server listening on port " << port << std::endl; - } - - ~UDPServer() { - if (sockfd >= 0) { - close(sockfd); - } - } - - std::vector receive() { - std::vector data(64); - - ssize_t received = recvfrom(sockfd, - data.data(), - data.size() * sizeof(uint16_t), - MSG_DONTWAIT, // Неблокирующий режим - (sockaddr*)&client_addr, - &client_len); - - if (received > 0) { - // Выводим информацию о отправителе и данные - data.resize(received / 2); - char client_ip[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &client_addr.sin_addr, client_ip, INET_ADDRSTRLEN); - // std::cout << "Received data from " << client_ip << ":" << ntohs(client_addr.sin_port) << std::endl; - - // std::cout << "Data: "; - // for (int i = 0; i < 4 && i < data.size(); ++i) { - // std::cout << data[i] << " "; - // } - // std::cout << "..." << std::endl; - - return data; - } - - return {}; - } -}; - -struct SbusData { - bool lost_frame = false; - bool failsafe = false; - bool ch17 = false, ch18 = false; - static constexpr size_t NUM_CH = 16; - int16_t ch[NUM_CH]; - - /* Message len */ - static constexpr int8_t BUF_LEN_ = 25; - /* SBUS message defs */ - static constexpr int8_t NUM_SBUS_CH_ = 16; - static constexpr uint8_t HEADER_ = 0x0F; - static constexpr uint8_t FOOTER_ = 0x00; - static constexpr uint8_t FOOTER2_ = 0x04; - static constexpr uint8_t CH17_MASK_ = 0x01; - static constexpr uint8_t CH18_MASK_ = 0x02; - static constexpr uint8_t LOST_FRAME_MASK_ = 0x04; - static constexpr uint8_t FAILSAFE_MASK_ = 0x08; - /* Data */ - uint8_t buf_[BUF_LEN_]; - - void fillDataBuf() { - /* Assemble packet */ - buf_[0] = HEADER_; - buf_[1] = static_cast((ch[0] & 0x07FF)); - buf_[2] = static_cast((ch[0] & 0x07FF) >> 8 | - (ch[1] & 0x07FF) << 3); - buf_[3] = static_cast((ch[1] & 0x07FF) >> 5 | - (ch[2] & 0x07FF) << 6); - buf_[4] = static_cast((ch[2] & 0x07FF) >> 2); - buf_[5] = static_cast((ch[2] & 0x07FF) >> 10 | - (ch[3] & 0x07FF) << 1); - buf_[6] = static_cast((ch[3] & 0x07FF) >> 7 | - (ch[4] & 0x07FF) << 4); - buf_[7] = static_cast((ch[4] & 0x07FF) >> 4 | - (ch[5] & 0x07FF) << 7); - buf_[8] = static_cast((ch[5] & 0x07FF) >> 1); - buf_[9] = static_cast((ch[5] & 0x07FF) >> 9 | - (ch[6] & 0x07FF) << 2); - buf_[10] = static_cast((ch[6] & 0x07FF) >> 6 | - (ch[7] & 0x07FF) << 5); - buf_[11] = static_cast((ch[7] & 0x07FF) >> 3); - buf_[12] = static_cast((ch[8] & 0x07FF)); - buf_[13] = static_cast((ch[8] & 0x07FF) >> 8 | - (ch[9] & 0x07FF) << 3); - buf_[14] = static_cast((ch[9] & 0x07FF) >> 5 | - (ch[10] & 0x07FF) << 6); - buf_[15] = static_cast((ch[10] & 0x07FF) >> 2); - buf_[16] = static_cast((ch[10] & 0x07FF) >> 10 | - (ch[11] & 0x07FF) << 1); - buf_[17] = static_cast((ch[11] & 0x07FF) >> 7 | - (ch[12] & 0x07FF) << 4); - buf_[18] = static_cast((ch[12] & 0x07FF) >> 4 | - (ch[13] & 0x07FF) << 7); - buf_[19] = static_cast((ch[13] & 0x07FF) >> 1); - buf_[20] = static_cast((ch[13] & 0x07FF) >> 9 | - (ch[14] & 0x07FF) << 2); - buf_[21] = static_cast((ch[14] & 0x07FF) >> 6 | - (ch[15] & 0x07FF) << 5); - buf_[22] = static_cast((ch[15] & 0x07FF) >> 3); - buf_[23] = 0x00 | (ch17 * CH17_MASK_) | (ch18 * CH18_MASK_) | - (failsafe * FAILSAFE_MASK_) | - (lost_frame * LOST_FRAME_MASK_); - buf_[24] = FOOTER_; - } - -}; - -class SerialPort { -private: - int fd; - -public: - SerialPort() : fd(-1) {} - - bool open(const std::string& port_path) { - // Открытие последовательного порта - fd = ::open(port_path.c_str(), O_RDWR | O_NOCTTY | O_SYNC); - if (fd < 0) { - std::cerr << "Failed to open serial port " << port_path << ": " << std::strerror(errno) << std::endl; - return false; - } - struct termios2 tio{}; - ioctl(fd, TCGETS2, &tio); - // 8bit - tio.c_cflag &= ~CSIZE; - tio.c_cflag |= CS8; - // even - tio.c_cflag &= ~(PARODD | CMSPAR); - tio.c_cflag |= PARENB; - // 2 stop bits - tio.c_cflag |= CSTOPB; - // baud rate - tio.c_ispeed = 100000; - tio.c_ospeed = 100000; - // other - tio.c_iflag |= (INPCK|IGNBRK|IGNCR|ISTRIP); - tio.c_cflag &= ~CBAUD; - tio.c_cflag |= (BOTHER|CREAD|CLOCAL); - - if (ioctl(fd, TCSETS2, &tio) != 0) { - std::cerr << "Failed to set termios2 attributes: " << std::strerror(errno) << std::endl; - close(fd); - return false; - } - - std::cout << "Serial port " << port_path << " opened and configured: 100000 baud, 8E2" << std::endl; - return true; - } - - ~SerialPort() { - _close(); - } - - void _close() { - if (fd >= 0) { - ::close(fd); - fd = -1; - } - } - - // Метод для получения файлового дескриптора порта - int getDescriptor() const { - return fd; - } - - // Метод для записи данных в порт - bool write(std::span data) { - if (fd < 0) return false; - - ssize_t written = ::write(fd, data.data(), data.size()); - if (written < 0) { - std::cerr << "Failed to write to serial port" << std::endl; - return false; - } - - // Принудительная отправка данных - // tcdrain(fd); - return true; - } -}; +static constexpr uint16_t UDP_PORT = 1067; int main(int argc, char* argv[]) { - // Парсим аргументы командной строки std::string serial_port = "/dev/ttyUSB0"; if (argc > 1) { serial_port = argv[1]; @@ -231,23 +15,16 @@ int main(int argc, char* argv[]) { try { // Создание UDP сервера - UDPServer udp_server(1066); - - // Открытие последовательного порта - SerialPort serial; - if (!serial.open(serial_port)) { - return 1; - } + drivers::UdpDriver udp_server(UDP_PORT); + drivers::UartDriver uart(serial_port, 416666); std::cout << "Ready to receive UDP packets and forward to serial port" << std::endl; std::cout << "Press Ctrl+C to exit" << std::endl; - SbusData sb{}; - int packet_count = 0; while (true) { // Прием UDP пакета - std::vector data = udp_server.receive(); + std::vector data = udp_server.recvPacket(); if (!data.empty()) { packet_count++; diff --git a/ground/joystick-reader.cpp b/ground/joystick-reader.cpp deleted file mode 100644 index cbac9be..0000000 --- a/ground/joystick-reader.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include "joystick-reader.h" -#include -#include -#include - -JoystickReader::JoystickReader() - : joystick(nullptr) {} - -JoystickReader::~JoystickReader() { - if (joystick) { - SDL_JoystickClose(joystick); - } - SDL_Quit(); -} - -bool JoystickReader::initialize() { - if (SDL_Init(SDL_INIT_JOYSTICK) < 0) { - std::cerr << "SDL initialization failed: " << SDL_GetError() << std::endl; - return false; - } - - int numJoysticks = SDL_NumJoysticks(); - if (numJoysticks < 1) { - std::cerr << "No joysticks found" << std::endl; - return false; - } - - std::cout << "Found " << numJoysticks << " joystick(s)" << std::endl; - - // Открываем первый джойстик - joystick = SDL_JoystickOpen(0); - if (!joystick) { - std::cerr << "Failed to open joystick: " << SDL_GetError() << std::endl; - return false; - } - - joystickName = SDL_JoystickName(joystick); - std::cout << "Opened joystick: " << joystickName << std::endl; - std::cout << "Axes: " << SDL_JoystickNumAxes(joystick) - << ", Buttons: " << SDL_JoystickNumButtons(joystick) - << ", Balls: " << SDL_JoystickNumBalls(joystick) - << ", Hats: " << SDL_JoystickNumHats(joystick) << std::endl; - - return true; -} - -bool JoystickReader::readData(std::vector& data) { - if (!joystick) return false; - if (SDL_JoystickGetAttached(joystick) != SDL_TRUE) { return false; } - - SDL_JoystickUpdate(); - - data.resize(24); // Заполняем нейтральным значением - for (auto& i: data) { i = 1500; } - - int axes = std::min(SDL_JoystickNumAxes(joystick), 8); - for (int i = 0; i < std::min(8, axes); ++i) { - Sint16 axisValue = SDL_JoystickGetAxis(joystick, i); - // Преобразуем из [-32768, 32767] в [1000, 2000] - data[i] = static_cast((axisValue + 32768) * 1000 / 65536 + 1000); - } - axes = 8; - - // Читаем кнопки - int buttons = SDL_JoystickNumButtons(joystick); - for (int i = 0; i < buttons && i < data.size() - axes; ++i) { - auto buttonState = SDL_JoystickGetButton(joystick, i); - data[axes + i] = static_cast(1000.0 + (buttonState * (1000.0 / 255.0))); - } - - for (auto& i: data) { - if (i < 950) i = 950; - if (i > 2050) i = 2050; - } - return true; -} - -std::string JoystickReader::getJoystickName() const { - return joystickName; -} diff --git a/ground/joystick-reader.h b/ground/joystick-reader.h deleted file mode 100644 index ced565a..0000000 --- a/ground/joystick-reader.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef SDRPI_FPV_CONTROL_GROUND_JOYSTICK_READER_H -#define SDRPI_FPV_CONTROL_GROUND_JOYSTICK_READER_H - -#include -#include -#include - -#ifdef _WIN32 -#include -#else -#include -#endif - -class JoystickReader { -public: - JoystickReader(); - ~JoystickReader(); - - bool initialize(); - bool readData(std::vector& data); - std::string getJoystickName() const; - -private: - SDL_Joystick* joystick; - std::string joystickName; -}; - -#endif //SDRPI_FPV_CONTROL_GROUND_JOYSTICK_READER_H \ No newline at end of file diff --git a/ground/main.cpp b/ground/main.cpp index c20fd4c..8a472ff 100644 --- a/ground/main.cpp +++ b/ground/main.cpp @@ -1,58 +1,86 @@ #include #include #include -#include +#include +#include +#include +#include -#include "joystick-reader.h" -#include "udp-driver.h" +#include "port/poller.h" +#include "port/uart.h" +#include "port/udp.h" + +static constexpr uint16_t UDP_PORT = 1067; int main(int argc, char* argv[]) { - - // Парсим аргументы командной строки для частоты - int frequency = 5; - const char* sendAddress; - if (argc == 1) { - sendAddress = "127.0.0.1"; - } else if (argc == 2) { - sendAddress = argv[1]; - } else if (argc == 3) { - sendAddress = argv[1]; - frequency = std::atoi(argv[2]); - } else { - std::cerr << "Usage: " << argv[0] << " send.ip.addr.v4 [Frequency]" << std::endl; + if (argc < 3 || argc > 4) { + std::cerr << "Usage: " << argv[0] + << " serial_port remote.ip.addr.4 [control packets frequency]\n"; return 1; } - std::cout << "Starting joystick reader with frequency: " << frequency << " Hz" << std::endl; + const std::string serialPort = argv[1]; + const std::string remoteIp = argv[2]; + const int controlFreq = (argc == 4) ? std::atoi(argv[3]) : 50; - JoystickReader reader; - const int64_t timeInterval = 1000 / frequency; - while (!reader.initialize()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - } + // UART speed fixed at 400k per your earlier specification + constexpr int UART_BAUD = 400000; - UDPSocket udp(sendAddress, 1066); - std::vector data; + std::cout << "Opening UART: " << serialPort << " @ " << UART_BAUD << "\n"; + std::cout << "Remote CRSF UDP endpoint: " << remoteIp << ":14550\n"; + std::cout << "Control packets frequency: " << controlFreq << " Hz\n"; - std::cout << "Sending data to " << sendAddress << ":1066" << std::endl; + poller::PollWrapper poller; - int counter = 0; - while (reader.readData(data)) { - if (!udp.sendFrame(data)) { - std::cerr << "Failed to send UDP packet" << std::endl; + auto uart = std::make_shared(serialPort, UART_BAUD); + auto udp = std::make_shared(UDP_PORT); + + poller.objects.push_back(uart); + poller.objects.push_back(udp); + + crsf::CrsfParser parser; + // CrsfTxProcessor processor(udp, uart, remoteIp, UDP_PORT, controlFreq); + + std::vector tmpBuffer; + + auto lastTick = std::chrono::steady_clock::now(); + + while (true) { + poller.loop(1000); + + if (uart->isPollHup()) { + std::cerr << "UART disconnected!\n"; + break; + } + if (udp->isPollHup()) { + std::cerr << "UDP socket error!\n"; + break; } - // Выводим прогресс каждые 100 пакетов - if (++counter % 100 == 0) { - std::cout << "Sent " << counter << " packets" << std::endl; + bool eventReceived = false; + + // UART input + if (uart->isPollIn()) { + eventReceived = true; + size_t read = uart->readChunk(tmpBuffer); + if (read > 0) { + parser.parseBytes(tmpBuffer); + } } - // вычислим время для сна - // должно быть время now+timeInterval - - std::this_thread::sleep_for(std::chrono::milliseconds(timeInterval)); + while (true) { + auto pkt = parser.pullPacket(); + if (pkt == nullptr) { + break; + } + pkt->writeToBuffer(tmpBuffer); + udp->sendTo(std::span(tmpBuffer), remoteIp, UDP_PORT); + } + + if (!eventReceived) { + std::cout << "[W]: No actions received in last 1s!" << std::endl; + } } - std::cout << "Exiting..." << std::endl; return 0; } diff --git a/ground/udp-driver.cpp b/ground/udp-driver.cpp deleted file mode 100644 index 627d89f..0000000 --- a/ground/udp-driver.cpp +++ /dev/null @@ -1,111 +0,0 @@ -#include "udp-driver.h" -#include -#include - -UDPSocket::UDPSocket(const std::string& ip, uint16_t port) { -#ifdef _WIN32 - sockfd = INVALID_SOCKET; -#else - sockfd = -1; -#endif - - if (!initialize()) { - return; - } - - // Создание сокета -#ifdef _WIN32 - sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (sockfd == INVALID_SOCKET) { - std::cerr << "Socket creation failed: " << WSAGetLastError() << std::endl; - return; - } -#else - sockfd = socket(AF_INET, SOCK_DGRAM, 0); - if (sockfd < 0) { - std::cerr << "Socket creation failed" << std::endl; - return; - } -#endif - - // Настройка адреса назначения - memset(&dest_addr, 0, sizeof(dest_addr)); - dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(port); - -#ifdef _WIN32 - dest_addr.sin_addr.s_addr = inet_addr(ip.c_str()); - if (dest_addr.sin_addr.s_addr == INADDR_NONE) { - std::cerr << "Invalid address: " << ip << std::endl; - closesocket(sockfd); - sockfd = INVALID_SOCKET; - } -#else - if (inet_pton(AF_INET, ip.c_str(), &dest_addr.sin_addr) <= 0) { - std::cerr << "Invalid address: " << ip << std::endl; - close(sockfd); - sockfd = -1; - } -#endif -} - -bool UDPSocket::initialize() { -#ifdef _WIN32 - // Инициализация Winsock - if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) { - std::cerr << "WSAStartup failed: " << WSAGetLastError() << std::endl; - return false; - } -#endif - return true; -} - -UDPSocket::~UDPSocket() { -#ifdef _WIN32 - if (sockfd != INVALID_SOCKET) { - closesocket(sockfd); - } - WSACleanup(); -#else - if (sockfd >= 0) { - close(sockfd); - } -#endif -} - -bool UDPSocket::sendFrame(const std::vector& data) { -#ifdef _WIN32 - if (sockfd == INVALID_SOCKET) return false; -#else - if (sockfd < 0) return false; -#endif - - if (data.empty()) return false; - - // Отправка данных -#ifdef _WIN32 - int result = sendto(sockfd, - reinterpret_cast(data.data()), - data.size() * sizeof(uint16_t), - 0, - (sockaddr*)&dest_addr, - sizeof(dest_addr)); - if (result == SOCKET_ERROR) { - std::cerr << "Send failed: " << WSAGetLastError() << std::endl; - return false; - } -#else - ssize_t result = sendto(sockfd, - data.data(), - data.size() * sizeof(uint16_t), - 0, - (sockaddr*)&dest_addr, - sizeof(dest_addr)); - if (result < 0) { - std::cerr << "Send failed" << std::endl; - return false; - } -#endif - - return true; -} diff --git a/ground/udp-driver.h b/ground/udp-driver.h deleted file mode 100644 index 072109a..0000000 --- a/ground/udp-driver.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef SDRPI_FPV_CONTROL_GROUND_UDP_DRIVER_H -#define SDRPI_FPV_CONTROL_GROUND_UDP_DRIVER_H - -#include -#include -#include - -#ifdef _WIN32 -#include -#include -#pragma comment(lib, "ws2_32.lib") -#else -#include -#include -#include -#include -#endif - -class UDPSocket { -public: - UDPSocket(const std::string& ip, uint16_t port); - ~UDPSocket(); - bool sendFrame(const std::vector& data); - -private: -#ifdef _WIN32 - SOCKET sockfd; - WSADATA wsaData; -#else - int sockfd; -#endif - sockaddr_in dest_addr; - - bool initialize(); -}; - -#endif //SDRPI_FPV_CONTROL_GROUND_UDP_DRIVER_H \ No newline at end of file diff --git a/lib/crsf.cpp b/lib/crsf.cpp new file mode 100644 index 0000000..62a99e5 --- /dev/null +++ b/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 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(payload.empty() + ? nullptr : &payload[0], payload.size() + 2)); + + // Нам нужно подготовить временный буфер, потому что адрес и тип не в payload + std::vector 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 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& dest) const { + dest.clear(); + + const uint8_t preamble = 0xC8; + const uint8_t size = static_cast(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 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& 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(std::span(buffer, bufferPos)); + if (frame->checkCrc()) { + frames.push_back(std::move(frame)); + } + + reset(); + } + } +} + +std::unique_ptr crsf::CrsfParser::pullPacket() { + std::unique_ptr out; + if (!this->frames.empty()) { + out = std::move(this->frames.front()); + frames.pop_front(); + } + return out; +} \ No newline at end of file diff --git a/lib/crsf.h b/lib/crsf.h new file mode 100644 index 0000000..013c108 --- /dev/null +++ b/lib/crsf.h @@ -0,0 +1,61 @@ +#ifndef SDRPI_FPV_CONTROL_PLATFORM_PORT_CRSF_H +#define SDRPI_FPV_CONTROL_PLATFORM_PORT_CRSF_H + +#include +#include +#include +#include +#include + +namespace crsf { + struct CrsfFrame { + uint8_t address; // адрес получателя + uint8_t type; // тип пакета + std::vector payload; // полезная нагрузка + uint8_t crc; // контрольная сумма + + CrsfFrame(); + CrsfFrame(uint8_t addr, uint8_t t, std::vector p); + CrsfFrame(std::span rawBuffer); + + bool checkCrc() const; + void setCrc(); + + void writeToBuffer(std::vector& dest) const; + }; + + constexpr size_t CRSF_MAX_FRAME_SIZE = 260; + + uint8_t crsfComputeCrc(std::span data); + + class CrsfParser { + public: + CrsfParser() = default; + + /** + * Разбирает поток байт и формирует пакеты, добавляя их во внутреннюю очередь. + * Метод может принимать чанки любых размеров (например, 512 байт с UART). + */ + void parseBytes(const std::vector& data); + + /** + * Отдает все готовые пакеты и очищает очередь. + */ + std::unique_ptr pullPacket(); + + void reset(); + + private: + static constexpr uint8_t PREAMBLE = 0xC8; + + uint8_t buffer[CRSF_MAX_FRAME_SIZE]{}; + size_t bufferPos{0}; + size_t frameSize{0}; + + std::deque> frames; + + }; + +} + +#endif //SDRPI_FPV_CONTROL_PLATFORM_PORT_CRSF_H \ No newline at end of file diff --git a/lib/port/poller-general.cpp b/lib/port/poller-general.cpp new file mode 100644 index 0000000..139597f --- /dev/null +++ b/lib/port/poller-general.cpp @@ -0,0 +1,2 @@ + + diff --git a/lib/port/poller.h b/lib/port/poller.h new file mode 100644 index 0000000..48fad10 --- /dev/null +++ b/lib/port/poller.h @@ -0,0 +1,54 @@ +#ifndef SDRPI_FPV_CONTROL_GROUND_PORT_POLLER_H +#define SDRPI_FPV_CONTROL_GROUND_PORT_POLLER_H + +#include +#include + +namespace poller { + /** + * PollObject - базовый объект для мониторинга I/O. + * Платформенно-специфичные дескрипторы хранятся в защищённых полях. + */ + class PollObject { + public: + bool isPollIn() const; + bool isPollOut() const; + bool isPollHup() const; + + virtual ~PollObject(); + protected: +#ifdef _WIN32 + // UART HANDLE или UDP событие + HANDLE hCom{INVALID_HANDLE_VALUE}; + SOCKET sock{INVALID_SOCKET}; + WSAEVENT winHandle{nullptr}; + short revents{0}; // событие произошло +#else + int fd{0}; + short events{0}; + short revents{0}; +#endif + friend class PollWrapper; + }; + + /** + * Класс-обертка для мониторинга файловых дискрипторов. Использует стек для хранения массива структур мониторинга. + */ + class PollWrapper { + public: + PollWrapper(); + + std::vector> objects; + + /** + * Функция, которую нужно вызывать в бесконечном цикле. Вызывает `poll`, после чего выполняет обработчики событий, если нужно. + * @param timeoutMs + */ + void loop(int timeoutMs = -1); + + ~PollWrapper(); + }; + +} // namespace poller + +#endif //SDRPI_FPV_CONTROL_GROUND_PORT_POLLER_H \ No newline at end of file diff --git a/lib/port/uart.h b/lib/port/uart.h new file mode 100644 index 0000000..6907ee1 --- /dev/null +++ b/lib/port/uart.h @@ -0,0 +1,25 @@ +#ifndef SDRPI_FPV_CONTROL_GROUND_PORT_UART_H +#define SDRPI_FPV_CONTROL_GROUND_PORT_UART_H +#include + +#include "port/poller.h" +#include +#include +#include + +namespace drivers { + class UartDriver : public poller::PollObject { + public: + UartDriver(const std::string& path, int baud); + bool writeData(std::span data); + size_t readChunk(std::vector& out); + ~UartDriver() override; + +#if _WIN32 + protected: + OVERLAPPED overlapped{}; +#endif + }; +} + +#endif //SDRPI_FPV_CONTROL_GROUND_PORT_UART_H diff --git a/lib/port/udp.h b/lib/port/udp.h new file mode 100644 index 0000000..f4140b5 --- /dev/null +++ b/lib/port/udp.h @@ -0,0 +1,21 @@ +#ifndef SDRPI_FPV_CONTROL_GROUND_PORT_UDP_H +#define SDRPI_FPV_CONTROL_GROUND_PORT_UDP_H + +#include "port/poller.h" +#include +#include +#include +#include + +namespace drivers { + class UdpDriver : public poller::PollObject { + public: + // port - локальный порт + explicit UdpDriver(uint16_t port); + bool sendTo(std::span data, const std::string& addr, uint16_t port); + bool recvPacket(std::vector& out); + ~UdpDriver() override; + }; +} + +#endif //SDRPI_FPV_CONTROL_GROUND_PORT_UDP_H \ No newline at end of file diff --git a/lib/port/unix/poller.cpp b/lib/port/unix/poller.cpp new file mode 100644 index 0000000..454c2e4 --- /dev/null +++ b/lib/port/unix/poller.cpp @@ -0,0 +1,51 @@ +#include "port/poller.h" + +#include +#include + +bool poller::PollObject::isPollIn() const { return revents & POLLIN; } +bool poller::PollObject::isPollOut() const { return revents & POLLOUT; } +bool poller::PollObject::isPollHup() const { return revents & POLLHUP; } +poller::PollObject::~PollObject() = default; + +poller::PollWrapper::PollWrapper() = default; + +void poller::PollWrapper::loop(int timeoutMs) { + if (this->objects.empty()) { + return; + } + + // проверяем, что нет объектов с fd < 0, удаляем такие объекты если они есть + for (size_t index = 0; index < this->objects.size();) { + if (this->objects[index]->fd < 0) { + this->objects.erase(this->objects.begin() + static_cast(index)); + } else { + index++; + } + } + + const auto qsize = this->objects.size(); + + // массив для вызова poll + pollfd pollFds[qsize]; + + // заполняем данные для poll + for (size_t i = 0; i < qsize; i++) { + pollFds[i].revents = 0; + pollFds[i].fd = this->objects[i]->fd; + pollFds[i].events = this->objects[i]->events; + } + + // выполняем poll + poll(pollFds, qsize, timeoutMs); + + // проверяем события + for (size_t i = 0; i < qsize; i++) { + objects[i]->revents = pollFds[i].revents; + // if (pollFds[i].revents != 0) { + // objects[i]->callback(); + // } + } +} + +poller::PollWrapper::~PollWrapper() = default; diff --git a/lib/port/unix/uart.cpp b/lib/port/unix/uart.cpp new file mode 100644 index 0000000..86b216a --- /dev/null +++ b/lib/port/unix/uart.cpp @@ -0,0 +1,60 @@ +#include "port/uart.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + + +drivers::UartDriver::UartDriver(const std::string& path, int baud) { + fd = open(path.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + if (fd < 0) throw std::runtime_error(std::string("UartDriver open error ") + path + ": " + std::strerror(errno)); + + struct termios2 tio{}; + if (ioctl(fd, TCGETS2, &tio) != 0) { + close(fd); + throw std::runtime_error(std::string("UartDriver setup error: ") + std::strerror(errno)); + } + // 8bit + tio.c_cflag &= ~CSIZE; + tio.c_cflag |= CS8; + // baud rate + tio.c_ispeed = baud; + tio.c_ospeed = baud; + // other + tio.c_iflag |= (INPCK|IGNBRK|IGNCR|ISTRIP); + tio.c_cflag &= ~CBAUD; + tio.c_cflag |= (BOTHER|CREAD|CLOCAL); + + if (ioctl(fd, TCSETS2, &tio) != 0) { + close(fd); + throw std::runtime_error(std::string("UartDriver setup error: ") + std::strerror(errno)); + } + + events = POLLIN; +} +bool drivers::UartDriver::writeData(std::span data) { + if (fd < 0) return false; + auto w = write(fd, data.data(), data.size()); + return w == data.size(); +} +size_t drivers::UartDriver::readChunk(std::vector& out) { + if (fd < 0) return 0; + out.resize(1024); + auto r = read(fd, out.data(), out.size()); + if (r <= 0) return 0; + out.resize(r); + return static_cast(r); +} +drivers::UartDriver::~UartDriver() { + close(fd); +} diff --git a/lib/port/unix/udp.cpp b/lib/port/unix/udp.cpp new file mode 100644 index 0000000..e111b80 --- /dev/null +++ b/lib/port/unix/udp.cpp @@ -0,0 +1,55 @@ +#include "port/udp.h" + +#include +#include +#include +#include +#include + +#include "crsf.h" + + +drivers::UdpDriver::UdpDriver(uint16_t port) { + fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) { + throw std::runtime_error(std::string("UdpDriver open error: ") + std::strerror(errno)); + } + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port); + addr.sin_addr.s_addr = INADDR_ANY; + + if (bind(fd, (sockaddr*)&addr, sizeof(addr)) < 0) if (fd < 0) { + close(fd); + throw std::runtime_error(std::string("UdpDriver bind error: ") + std::strerror(errno)); + } + + events = POLLIN; +} +bool drivers::UdpDriver::sendTo(std::span data, const std::string& addr, uint16_t port) { + if (fd < 0) return false; + + sockaddr_in dst{}; + dst.sin_family = AF_INET; + dst.sin_port = htons(port); + inet_aton(addr.c_str(), &dst.sin_addr); + + auto w = sendto(fd, data.data(), data.size(), 0, (sockaddr*)&dst, sizeof(dst)); + return w == data.size(); +} +bool drivers::UdpDriver::recvPacket(std::vector& out) { + out.resize(crsf::CRSF_MAX_FRAME_SIZE); + ssize_t received = recvfrom(fd, out.data(), out.size(), MSG_DONTWAIT, nullptr, nullptr); + + if (received > 0) { + out.resize(received); + return true; + } + out.clear(); + + return false; +} +drivers::UdpDriver::~UdpDriver() { + close(fd); +} diff --git a/lib/port/win/poller.cpp b/lib/port/win/poller.cpp new file mode 100644 index 0000000..8aee0f4 --- /dev/null +++ b/lib/port/win/poller.cpp @@ -0,0 +1,64 @@ +#include "port/poller.h" +#include +#include + +bool poller::PollObject::isPollIn() const { return revents != 0; } +bool poller::PollObject::isPollOut() const { return true; } // для Windows драйверов обычно всегда можно писать +bool poller::PollObject::isPollHup() const { + // UART: проверка ClearCommError + if (hCom != INVALID_HANDLE_VALUE) { + DWORD errors; + if (!ClearCommError(hCom, &errors, nullptr)) { + DWORD err = GetLastError(); + if (err == ERROR_INVALID_HANDLE || err == ERROR_FILE_NOT_FOUND) + return true; // COM порт физически пропал + } + if (errors & CE_RXOVER) return true; // overflow тоже можно трактовать как проблему + } + + // UDP: проверка ошибки сокета через SO_ERROR + if (sock != INVALID_SOCKET) { + int err = 0; + int len = sizeof(err); + if (getsockopt(sock, SOL_SOCKET, SO_ERROR, (char*)&err, &len) == 0) { + if (err != 0) return true; // есть ошибка на сокете + } + } + + return false; +} +poller::PollObject::~PollObject() = default; + +poller::PollWrapper::PollWrapper() = default; + +void poller::PollWrapper::loop(int timeoutMs) { + // собираем все события + std::vector handles; + for (auto& obj : objects) { +#ifdef _WIN32 + if (!obj) continue; + // UART HANDLE или UDP событие + if (obj->winHandle) + handles.push_back(obj->winHandle); +#endif + } + + if (handles.empty()) return; + + DWORD waitTime = (timeoutMs < 0) ? INFINITE : static_cast(timeoutMs); + DWORD rc = WaitForMultipleObjects(static_cast(handles.size()), + handles.data(), + FALSE, // ждем любого события + waitTime); + if (rc == WAIT_FAILED) + throw std::runtime_error("WaitForMultipleObjects failed"); + + // Определяем, какой объект сработал + int idx = rc - WAIT_OBJECT_0; + if (idx >= 0 && idx < static_cast(handles.size())) { + auto& obj = objects[idx]; + obj->revents = 1; // простая метка, что событие произошло + } +} + +poller::PollWrapper::~PollWrapper() = default; diff --git a/lib/port/win/uart.cpp b/lib/port/win/uart.cpp new file mode 100644 index 0000000..c6bba12 --- /dev/null +++ b/lib/port/win/uart.cpp @@ -0,0 +1,71 @@ +#include "port/poller.h" +#include "port/uart.h" +#include +#include +#include +#include +#include + +drivers::UartDriver::UartDriver(const std::string& path, int baud) { + hCom = CreateFileA(path.c_str(), + GENERIC_READ | GENERIC_WRITE, + 0, nullptr, + OPEN_EXISTING, + FILE_FLAG_OVERLAPPED, + nullptr); + if (hCom == INVALID_HANDLE_VALUE) + throw std::runtime_error("Cannot open UART port"); + + // Настройка DCB + DCB dcb{}; + dcb.DCBlength = sizeof(dcb); + if (!GetCommState(hCom, &dcb)) + throw std::runtime_error("GetCommState failed"); + + dcb.BaudRate = baud; + dcb.ByteSize = 8; + dcb.Parity = EVENPARITY; // 8E2 + dcb.StopBits = TWOSTOPBITS; + + if (!SetCommState(hCom, &dcb)) + throw std::runtime_error("SetCommState failed"); + + // Настройка событий + winHandle = CreateEvent(nullptr, TRUE, FALSE, nullptr); + if (!winHandle) throw std::runtime_error("CreateEvent failed"); + + if (!SetCommMask(hCom, EV_RXCHAR)) + throw std::runtime_error("SetCommMask failed"); + + // Первый вызов WaitCommEvent в асинхронном режиме + memset(&overlapped, 0, sizeof(overlapped)); + overlapped.hEvent = winHandle; + DWORD dummy; + WaitCommEvent(hCom, &dummy, &overlapped); +} +bool drivers::UartDriver::writeData(std::span data) { + DWORD written; + OVERLAPPED ov{}; + ov.hEvent = CreateEvent(nullptr, TRUE, FALSE, nullptr); + + bool ok = WriteFile(hCom, data.data(), (DWORD)data.size(), &written, &ov) || + GetOverlappedResult(hCom, &ov, &written, TRUE); + CloseHandle(ov.hEvent); + return ok && written == data.size(); +} +size_t drivers::UartDriver::readChunk(std::vector& out) { + out.resize(512); + DWORD read = 0; + OVERLAPPED ov{}; + ov.hEvent = CreateEvent(nullptr, TRUE, FALSE, nullptr); + bool ok = ReadFile(hCom, out.data(), (DWORD)out.size(), &read, &ov) || + GetOverlappedResult(hCom, &ov, &read, TRUE); + CloseHandle(ov.hEvent); + out.resize(read); + return read; +} +drivers::UartDriver::~UartDriver() { + if (hCom != INVALID_HANDLE_VALUE) CloseHandle(hCom); + if (winHandle) CloseHandle(winHandle); +} + diff --git a/lib/port/win/udp.cpp b/lib/port/win/udp.cpp new file mode 100644 index 0000000..7560c90 --- /dev/null +++ b/lib/port/win/udp.cpp @@ -0,0 +1,66 @@ +// File: src/port/win/udp_driver_win.cpp +#include "pollobject.h" +#include +#include +#include +#include +#include +#include + +#pragma comment(lib, "Ws2_32.lib") + +namespace drivers { + + class UdpDriver : public poller::PollObject { + public: + explicit UdpDriver(uint16_t port) { + WSADATA wsa; + WSAStartup(MAKEWORD(2,2), &wsa); + + sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (sock == INVALID_SOCKET) throw std::runtime_error("Cannot create socket"); + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port); + addr.sin_addr.s_addr = INADDR_ANY; + + if (bind(sock, (sockaddr*)&addr, sizeof(addr)) == SOCKET_ERROR) + throw std::runtime_error("Bind failed"); + + winHandle = WSACreateEvent(); + if (!winHandle) throw std::runtime_error("WSACreateEvent failed"); + + WSAEventSelect(sock, winHandle, FD_READ); + } + + bool sendTo(std::span data, const std::string& addrStr, uint16_t port) { + sockaddr_in dest{}; + dest.sin_family = AF_INET; + dest.sin_port = htons(port); + inet_pton(AF_INET, addrStr.c_str(), &dest.sin_addr); + + int ret = sendto(sock, (const char*)data.data(), (int)data.size(), 0, + (sockaddr*)&dest, sizeof(dest)); + return ret == (int)data.size(); + } + + bool recvPacket(std::vector& out) { + out.resize(512); + sockaddr_in src{}; + int len = sizeof(src); + int ret = recvfrom(sock, (char*)out.data(), (int)out.size(), MSG_PEEK, + (sockaddr*)&src, &len); + if (ret <= 0) return false; // ничего нет или ошибка + out.resize(ret); + return true; + } + + ~UdpDriver() override { + if (sock != INVALID_SOCKET) closesocket(sock); + if (winHandle) WSACloseEvent(winHandle); + WSACleanup(); + } + }; + +} // namespace drivers diff --git a/vcpkg.json b/vcpkg.json deleted file mode 100644 index 1b00a09..0000000 --- a/vcpkg.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "dependencies": [ - "sdl2" - ] -} \ No newline at end of file