From af351135a2273dabbe35a893f21440f676d05c26 Mon Sep 17 00:00:00 2001 From: Vladislav Ostapov Date: Thu, 13 Nov 2025 19:13:53 +0300 Subject: [PATCH] =?UTF-8?q?=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2=D0=B8=D0=BB=20?= =?UTF-8?q?=D0=BF=D1=80=D0=BE=D0=B3=D1=80=D0=B0=D0=BC=D0=BC=D1=83=20=D0=B4?= =?UTF-8?q?=D0=BB=D1=8F=20=D0=B2=D0=BE=D0=B7=D0=B4=D1=83=D1=85=D0=B0,=20?= =?UTF-8?q?=D0=BD=D0=BE=20=D0=BE=D0=BD=D0=B0=20=D0=BF=D0=BE=D0=BA=D0=B0=20?= =?UTF-8?q?=D0=BD=D0=B5=20=D1=80=D0=B0=D0=B1=D0=BE=D1=82=D0=B0=D0=B5=D1=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 7 +- air/main.cpp | 295 +++++++++++++++++++++++++++++++++++++ ground/joystick-reader.cpp | 2 +- ground/main.cpp | 2 +- 4 files changed, 300 insertions(+), 6 deletions(-) create mode 100644 air/main.cpp diff --git a/README.md b/README.md index 19bc5c3..8250d8c 100644 --- a/README.md +++ b/README.md @@ -9,17 +9,16 @@ ## Linux ```shell -sudo apt install libhidapi-dev +sudo apt install libsdl2-dev ``` ## MacOS ```shell -brew install hidapi +brew install sdl2 ``` ## Шиндовс -Под шиндовс, как всегда, надо страдать. Рекомендуется установить `libusb` и `hidapi` через `vcpkg`. +Под шиндовс, как всегда, надо страдать. Рекомендуется установить `sdl2` через `vcpkg`. -# diff --git a/air/main.cpp b/air/main.cpp new file mode 100644 index 0000000..9c00238 --- /dev/null +++ b/air/main.cpp @@ -0,0 +1,295 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + + + + +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(16); + + ssize_t received = recvfrom(sockfd, + data.data(), + data.size() * sizeof(uint16_t), + MSG_DONTWAIT, // Неблокирующий режим + (sockaddr*)&client_addr, + &client_len); + + if (received > 0) { + if (received == 16 * sizeof(uint16_t)) { + // Выводим информацию о отправителе и данные + 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; + } else { + std::cerr << "Invalid packet size: " << received << " bytes" << std::endl; + } + } + + 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 tty2; + if (ioctl(fd, TCGETS2, &tty2) != 0) { + std::cerr << "Failed to get termios2 attributes: " << std::strerror(errno) << std::endl; + close(fd); + return false; + } + + // Отключаем все флаги управления потоком и прочие опции + tty2.c_cflag &= ~CSIZE; + tty2.c_cflag |= CS8; // 8 бит данных + tty2.c_cflag |= PARENB; // включаем четность + tty2.c_cflag &= ~PARODD; // even parity + tty2.c_cflag |= CSTOPB; // 2 стоп-бита + tty2.c_cflag |= (CLOCAL | CREAD); + + tty2.c_iflag &= ~(IXON | IXOFF | IXANY | INLCR | ICRNL); + tty2.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + tty2.c_oflag &= ~OPOST; + + tty2.c_cc[VMIN] = 0; + tty2.c_cc[VTIME] = 5; + + // Устанавливаем нестандартную скорость + tty2.c_cflag &= ~CBAUD; + tty2.c_cflag |= BOTHER; + tty2.c_ispeed = 100000; + tty2.c_ospeed = 100000; + + if (ioctl(fd, TCSETS2, &tty2) != 0) { + std::cerr << "Failed to set termios2 attributes: " << std::strerror(errno) << std::endl; + close(fd); + return false; + } + + // tcflush(fd, TCIOFLUSH); + + 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; + } +}; + +int main(int argc, char* argv[]) { + // Парсим аргументы командной строки + std::string serial_port = "/dev/ttyUSB0"; + if (argc > 1) { + serial_port = argv[1]; + } + + try { + // Создание UDP сервера + UDPServer udp_server(1066); + + // Открытие последовательного порта + SerialPort serial; + if (!serial.open(serial_port)) { + return 1; + } + + 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(); + + if (!data.empty()) { + packet_count++; + + for (int i = 0; i < data.size() && i < sb.NUM_CH; ++i) { + sb.ch[i] = static_cast(data[i]); + } + sb.fillDataBuf(); + serial.write(sb.buf_); + + // Выводим статистику каждые 100 пакетов + if (packet_count % 100 == 0) { + std::cout << "Received " << packet_count << " packets total" << std::endl; + } + } + + // Небольшая пауза чтобы не грузить CPU + usleep(1000); // 1 мс + } + + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + return 1; + } + + std::cout << "Exiting..." << std::endl; + return 0; +} + diff --git a/ground/joystick-reader.cpp b/ground/joystick-reader.cpp index a498495..5039d3a 100644 --- a/ground/joystick-reader.cpp +++ b/ground/joystick-reader.cpp @@ -69,7 +69,7 @@ bool JoystickReader::readData(std::vector& data) { for (auto& i: data) { if (i < 900) i = 900; - if (i > 1100) i = 1100; + if (i > 2100) i = 2100; } std::this_thread::sleep_for(std::chrono::milliseconds(updateIntervalMs)); diff --git a/ground/main.cpp b/ground/main.cpp index 2b7c3db..4990e59 100644 --- a/ground/main.cpp +++ b/ground/main.cpp @@ -9,7 +9,7 @@ int main(int argc, char* argv[]) { // Парсим аргументы командной строки для частоты - int frequency = 50; + int frequency = 5; const char* sendAddress; if (argc == 1) { sendAddress = "127.0.0.1";