From 6ef59210bb5bb7a76fd3df75f7cc232133ca95fa Mon Sep 17 00:00:00 2001 From: Vladislav Ostapov Date: Thu, 13 Nov 2025 18:20:00 +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=B2=D1=81=D0=B5=20=D0=B4=D0=BB=D1=8F=20=D0=BD=D0=B0=D0=B7?= =?UTF-8?q?=D0=B5=D0=BC=D0=BD=D0=BE=D0=B9=20=D1=81=D1=82=D0=B0=D0=BD=D1=86?= =?UTF-8?q?=D0=B8=D0=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 3 + CMakeLists.txt | 38 +++++++++++++ README.md | 25 +++++++++ ground/joystick-reader.cpp | 81 +++++++++++++++++++++++++++ ground/joystick-reader.h | 29 ++++++++++ ground/main.cpp | 52 +++++++++++++++++ ground/udp-driver.cpp | 111 +++++++++++++++++++++++++++++++++++++ ground/udp-driver.h | 37 +++++++++++++ 8 files changed, 376 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 ground/joystick-reader.cpp create mode 100644 ground/joystick-reader.h create mode 100644 ground/main.cpp create mode 100644 ground/udp-driver.cpp create mode 100644 ground/udp-driver.h diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..6f52371 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +cmake-build-* +.idea + diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..73582f9 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.28) +project(sdrpi-fpv-control) + +set(CMAKE_CXX_STANDARD 20) + +option(ENABLE_GROUND_BUILD "Enable build ground station" OFF) +option(ENABLE_AIR_BUILD "Enable build air" ON) + +if (ENABLE_GROUND_BUILD) + message("Enabled ground build!") + # Настройки для Windows + if(WIN32) + add_definitions(-DWIN32_LEAN_AND_MEAN) + set(EXTRA_LIBS ws2_32) + 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 + ) + + target_link_libraries(${PROJECT_NAME}-ground ${SDL2_LIBRARIES} ${EXTRA_LIBS}) +endif() + +if (ENABLE_AIR_BUILD) + message("Enabled air build!") + + add_executable(${PROJECT_NAME}-air + air/main.cpp + ) +endif() + diff --git a/README.md b/README.md new file mode 100644 index 0000000..19bc5c3 --- /dev/null +++ b/README.md @@ -0,0 +1,25 @@ +# SDRPi fpv control + +Это проект с двумя частями программы: +* программа `ground` - запускается на наземной станции + + +# Зависимости для ground + +## Linux + +```shell +sudo apt install libhidapi-dev +``` + +## MacOS + +```shell +brew install hidapi +``` + +## Шиндовс + +Под шиндовс, как всегда, надо страдать. Рекомендуется установить `libusb` и `hidapi` через `vcpkg`. + +# diff --git a/ground/joystick-reader.cpp b/ground/joystick-reader.cpp new file mode 100644 index 0000000..a498495 --- /dev/null +++ b/ground/joystick-reader.cpp @@ -0,0 +1,81 @@ +#include "joystick-reader.h" +#include +#include +#include + +JoystickReader::JoystickReader(int frequency) + : joystick(nullptr), updateIntervalMs(1000 / frequency) {} + +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(16, 1500); // Заполняем нейтральным значением + + // Читаем оси (обычно до 6 осей) + int axes = std::min(SDL_JoystickNumAxes(joystick), 6); + for (int i = 0; i < axes; ++i) { + Sint16 axisValue = SDL_JoystickGetAxis(joystick, i); + // Преобразуем из [-32768, 32767] в [1000, 2000] + data[i] = static_cast((axisValue + 32768) * 1000 / 65536 + 1000); + } + + // Читаем кнопки + int buttons = SDL_JoystickNumButtons(joystick); + for (int i = 0; i < buttons && i < data.size() - axes; ++i) { + Uint8 buttonState = SDL_JoystickGetButton(joystick, i); + data[axes + i] = buttonState ? 2000 : 1000; + } + + for (auto& i: data) { + if (i < 900) i = 900; + if (i > 1100) i = 1100; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(updateIntervalMs)); + return true; +} + +std::string JoystickReader::getJoystickName() const { + return joystickName; +} diff --git a/ground/joystick-reader.h b/ground/joystick-reader.h new file mode 100644 index 0000000..45df640 --- /dev/null +++ b/ground/joystick-reader.h @@ -0,0 +1,29 @@ +#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(int frequency = 50); + ~JoystickReader(); + + bool initialize(); + bool readData(std::vector& data); + std::string getJoystickName() const; + +private: + SDL_Joystick* joystick; + int updateIntervalMs; + 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 new file mode 100644 index 0000000..2b7c3db --- /dev/null +++ b/ground/main.cpp @@ -0,0 +1,52 @@ +#include +#include +#include +#include + +#include "joystick-reader.h" +#include "udp-driver.h" + +int main(int argc, char* argv[]) { + + // Парсим аргументы командной строки для частоты + int frequency = 50; + 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; + return 1; + } + + std::cout << "Starting joystick reader with frequency: " << frequency << " Hz" << std::endl; + + JoystickReader reader(frequency); + while (!reader.initialize()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + + UDPSocket udp(sendAddress, 1066); + std::vector data; + + std::cout << "Sending data to " << sendAddress << ":1066" << std::endl; + + int counter = 0; + while (reader.readData(data)) { + if (!udp.sendFrame(data)) { + std::cerr << "Failed to send UDP packet" << std::endl; + } + + // Выводим прогресс каждые 100 пакетов + if (++counter % 100 == 0) { + std::cout << "Sent " << counter << " packets" << std::endl; + } + } + + std::cout << "Exiting..." << std::endl; + return 0; +} diff --git a/ground/udp-driver.cpp b/ground/udp-driver.cpp new file mode 100644 index 0000000..627d89f --- /dev/null +++ b/ground/udp-driver.cpp @@ -0,0 +1,111 @@ +#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 new file mode 100644 index 0000000..072109a --- /dev/null +++ b/ground/udp-driver.h @@ -0,0 +1,37 @@ +#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