Compare commits
1 Commits
dev-sbus-t
...
dev-crsf
| Author | SHA1 | Date | |
|---|---|---|---|
| 3eaea1b966 |
@@ -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()
|
||||
|
||||
|
||||
235
air/main.cpp
235
air/main.cpp
@@ -1,229 +1,13 @@
|
||||
#include <sys/ioctl.h>
|
||||
#include <asm/termbits.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <cstring>
|
||||
#include <atomic>
|
||||
#include <span>
|
||||
#include <port/udp.h>
|
||||
|
||||
#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<uint16_t> receive() {
|
||||
std::vector<uint16_t> 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<uint8_t>((ch[0] & 0x07FF));
|
||||
buf_[2] = static_cast<uint8_t>((ch[0] & 0x07FF) >> 8 |
|
||||
(ch[1] & 0x07FF) << 3);
|
||||
buf_[3] = static_cast<uint8_t>((ch[1] & 0x07FF) >> 5 |
|
||||
(ch[2] & 0x07FF) << 6);
|
||||
buf_[4] = static_cast<uint8_t>((ch[2] & 0x07FF) >> 2);
|
||||
buf_[5] = static_cast<uint8_t>((ch[2] & 0x07FF) >> 10 |
|
||||
(ch[3] & 0x07FF) << 1);
|
||||
buf_[6] = static_cast<uint8_t>((ch[3] & 0x07FF) >> 7 |
|
||||
(ch[4] & 0x07FF) << 4);
|
||||
buf_[7] = static_cast<uint8_t>((ch[4] & 0x07FF) >> 4 |
|
||||
(ch[5] & 0x07FF) << 7);
|
||||
buf_[8] = static_cast<uint8_t>((ch[5] & 0x07FF) >> 1);
|
||||
buf_[9] = static_cast<uint8_t>((ch[5] & 0x07FF) >> 9 |
|
||||
(ch[6] & 0x07FF) << 2);
|
||||
buf_[10] = static_cast<uint8_t>((ch[6] & 0x07FF) >> 6 |
|
||||
(ch[7] & 0x07FF) << 5);
|
||||
buf_[11] = static_cast<uint8_t>((ch[7] & 0x07FF) >> 3);
|
||||
buf_[12] = static_cast<uint8_t>((ch[8] & 0x07FF));
|
||||
buf_[13] = static_cast<uint8_t>((ch[8] & 0x07FF) >> 8 |
|
||||
(ch[9] & 0x07FF) << 3);
|
||||
buf_[14] = static_cast<uint8_t>((ch[9] & 0x07FF) >> 5 |
|
||||
(ch[10] & 0x07FF) << 6);
|
||||
buf_[15] = static_cast<uint8_t>((ch[10] & 0x07FF) >> 2);
|
||||
buf_[16] = static_cast<uint8_t>((ch[10] & 0x07FF) >> 10 |
|
||||
(ch[11] & 0x07FF) << 1);
|
||||
buf_[17] = static_cast<uint8_t>((ch[11] & 0x07FF) >> 7 |
|
||||
(ch[12] & 0x07FF) << 4);
|
||||
buf_[18] = static_cast<uint8_t>((ch[12] & 0x07FF) >> 4 |
|
||||
(ch[13] & 0x07FF) << 7);
|
||||
buf_[19] = static_cast<uint8_t>((ch[13] & 0x07FF) >> 1);
|
||||
buf_[20] = static_cast<uint8_t>((ch[13] & 0x07FF) >> 9 |
|
||||
(ch[14] & 0x07FF) << 2);
|
||||
buf_[21] = static_cast<uint8_t>((ch[14] & 0x07FF) >> 6 |
|
||||
(ch[15] & 0x07FF) << 5);
|
||||
buf_[22] = static_cast<uint8_t>((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<const uint8_t> 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<uint16_t> data = udp_server.receive();
|
||||
std::vector<uint16_t> data = udp_server.recvPacket();
|
||||
|
||||
if (!data.empty()) {
|
||||
packet_count++;
|
||||
|
||||
@@ -1,80 +0,0 @@
|
||||
#include "joystick-reader.h"
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
|
||||
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<uint16_t>& 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<uint16_t>((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<uint16_t>(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;
|
||||
}
|
||||
@@ -1,28 +0,0 @@
|
||||
#ifndef SDRPI_FPV_CONTROL_GROUND_JOYSTICK_READER_H
|
||||
#define SDRPI_FPV_CONTROL_GROUND_JOYSTICK_READER_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <SDL.h>
|
||||
#else
|
||||
#include <SDL2/SDL.h>
|
||||
#endif
|
||||
|
||||
class JoystickReader {
|
||||
public:
|
||||
JoystickReader();
|
||||
~JoystickReader();
|
||||
|
||||
bool initialize();
|
||||
bool readData(std::vector<uint16_t>& data);
|
||||
std::string getJoystickName() const;
|
||||
|
||||
private:
|
||||
SDL_Joystick* joystick;
|
||||
std::string joystickName;
|
||||
};
|
||||
|
||||
#endif //SDRPI_FPV_CONTROL_GROUND_JOYSTICK_READER_H
|
||||
100
ground/main.cpp
100
ground/main.cpp
@@ -1,58 +1,86 @@
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <thread>
|
||||
#include <csignal>
|
||||
#include <port/poller.h>
|
||||
#include <port/uart.h>
|
||||
#include <port/udp.h>
|
||||
#include <crsf.h>
|
||||
|
||||
#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<uint16_t> 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<drivers::UartDriver>(serialPort, UART_BAUD);
|
||||
auto udp = std::make_shared<drivers::UdpDriver>(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<uint8_t> 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
|
||||
while (true) {
|
||||
auto pkt = parser.pullPacket();
|
||||
if (pkt == nullptr) {
|
||||
break;
|
||||
}
|
||||
pkt->writeToBuffer(tmpBuffer);
|
||||
udp->sendTo(std::span<const uint8_t>(tmpBuffer), remoteIp, UDP_PORT);
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(timeInterval));
|
||||
if (!eventReceived) {
|
||||
std::cout << "[W]: No actions received in last 1s!" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << "Exiting..." << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -1,111 +0,0 @@
|
||||
#include "udp-driver.h"
|
||||
#include <iostream>
|
||||
#include <cstring>
|
||||
|
||||
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<uint16_t>& 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<const char*>(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;
|
||||
}
|
||||
@@ -1,37 +0,0 @@
|
||||
#ifndef SDRPI_FPV_CONTROL_GROUND_UDP_DRIVER_H
|
||||
#define SDRPI_FPV_CONTROL_GROUND_UDP_DRIVER_H
|
||||
|
||||
#include <string>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <winsock2.h>
|
||||
#include <ws2tcpip.h>
|
||||
#pragma comment(lib, "ws2_32.lib")
|
||||
#else
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <unistd.h>
|
||||
#endif
|
||||
|
||||
class UDPSocket {
|
||||
public:
|
||||
UDPSocket(const std::string& ip, uint16_t port);
|
||||
~UDPSocket();
|
||||
bool sendFrame(const std::vector<uint16_t>& 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
|
||||
138
lib/crsf.cpp
Normal file
138
lib/crsf.cpp
Normal file
@@ -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;
|
||||
}
|
||||
61
lib/crsf.h
Normal file
61
lib/crsf.h
Normal file
@@ -0,0 +1,61 @@
|
||||
#ifndef SDRPI_FPV_CONTROL_PLATFORM_PORT_CRSF_H
|
||||
#define SDRPI_FPV_CONTROL_PLATFORM_PORT_CRSF_H
|
||||
|
||||
#include <algorithm>
|
||||
#include <deque>
|
||||
#include <memory>
|
||||
#include <span>
|
||||
#include <vector>
|
||||
|
||||
namespace crsf {
|
||||
struct CrsfFrame {
|
||||
uint8_t address; // адрес получателя
|
||||
uint8_t type; // тип пакета
|
||||
std::vector<uint8_t> payload; // полезная нагрузка
|
||||
uint8_t crc; // контрольная сумма
|
||||
|
||||
CrsfFrame();
|
||||
CrsfFrame(uint8_t addr, uint8_t t, std::vector<uint8_t> p);
|
||||
CrsfFrame(std::span<const uint8_t> rawBuffer);
|
||||
|
||||
bool checkCrc() const;
|
||||
void setCrc();
|
||||
|
||||
void writeToBuffer(std::vector<uint8_t>& dest) const;
|
||||
};
|
||||
|
||||
constexpr size_t CRSF_MAX_FRAME_SIZE = 260;
|
||||
|
||||
uint8_t crsfComputeCrc(std::span<const uint8_t> data);
|
||||
|
||||
class CrsfParser {
|
||||
public:
|
||||
CrsfParser() = default;
|
||||
|
||||
/**
|
||||
* Разбирает поток байт и формирует пакеты, добавляя их во внутреннюю очередь.
|
||||
* Метод может принимать чанки любых размеров (например, 512 байт с UART).
|
||||
*/
|
||||
void parseBytes(const std::vector<uint8_t>& data);
|
||||
|
||||
/**
|
||||
* Отдает все готовые пакеты и очищает очередь.
|
||||
*/
|
||||
std::unique_ptr<CrsfFrame> 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<std::unique_ptr<CrsfFrame>> frames;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif //SDRPI_FPV_CONTROL_PLATFORM_PORT_CRSF_H
|
||||
2
lib/port/poller-general.cpp
Normal file
2
lib/port/poller-general.cpp
Normal file
@@ -0,0 +1,2 @@
|
||||
|
||||
|
||||
54
lib/port/poller.h
Normal file
54
lib/port/poller.h
Normal file
@@ -0,0 +1,54 @@
|
||||
#ifndef SDRPI_FPV_CONTROL_GROUND_PORT_POLLER_H
|
||||
#define SDRPI_FPV_CONTROL_GROUND_PORT_POLLER_H
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
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<std::shared_ptr<PollObject>> objects;
|
||||
|
||||
/**
|
||||
* Функция, которую нужно вызывать в бесконечном цикле. Вызывает `poll`, после чего выполняет обработчики событий, если нужно.
|
||||
* @param timeoutMs
|
||||
*/
|
||||
void loop(int timeoutMs = -1);
|
||||
|
||||
~PollWrapper();
|
||||
};
|
||||
|
||||
} // namespace poller
|
||||
|
||||
#endif //SDRPI_FPV_CONTROL_GROUND_PORT_POLLER_H
|
||||
25
lib/port/uart.h
Normal file
25
lib/port/uart.h
Normal file
@@ -0,0 +1,25 @@
|
||||
#ifndef SDRPI_FPV_CONTROL_GROUND_PORT_UART_H
|
||||
#define SDRPI_FPV_CONTROL_GROUND_PORT_UART_H
|
||||
#include <cstdint>
|
||||
|
||||
#include "port/poller.h"
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <span>
|
||||
|
||||
namespace drivers {
|
||||
class UartDriver : public poller::PollObject {
|
||||
public:
|
||||
UartDriver(const std::string& path, int baud);
|
||||
bool writeData(std::span<const uint8_t> data);
|
||||
size_t readChunk(std::vector<uint8_t>& out);
|
||||
~UartDriver() override;
|
||||
|
||||
#if _WIN32
|
||||
protected:
|
||||
OVERLAPPED overlapped{};
|
||||
#endif
|
||||
};
|
||||
}
|
||||
|
||||
#endif //SDRPI_FPV_CONTROL_GROUND_PORT_UART_H
|
||||
21
lib/port/udp.h
Normal file
21
lib/port/udp.h
Normal file
@@ -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 <string>
|
||||
#include <vector>
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
|
||||
namespace drivers {
|
||||
class UdpDriver : public poller::PollObject {
|
||||
public:
|
||||
// port - локальный порт
|
||||
explicit UdpDriver(uint16_t port);
|
||||
bool sendTo(std::span<const uint8_t> data, const std::string& addr, uint16_t port);
|
||||
bool recvPacket(std::vector<uint8_t>& out);
|
||||
~UdpDriver() override;
|
||||
};
|
||||
}
|
||||
|
||||
#endif //SDRPI_FPV_CONTROL_GROUND_PORT_UDP_H
|
||||
51
lib/port/unix/poller.cpp
Normal file
51
lib/port/unix/poller.cpp
Normal file
@@ -0,0 +1,51 @@
|
||||
#include "port/poller.h"
|
||||
|
||||
#include <sys/poll.h>
|
||||
#include <algorithm>
|
||||
|
||||
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<ssize_t>(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;
|
||||
60
lib/port/unix/uart.cpp
Normal file
60
lib/port/unix/uart.cpp
Normal file
@@ -0,0 +1,60 @@
|
||||
#include "port/uart.h"
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <asm/termbits.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <cstring>
|
||||
#include <atomic>
|
||||
#include <span>
|
||||
|
||||
|
||||
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<const uint8_t> 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<uint8_t>& 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<size_t>(r);
|
||||
}
|
||||
drivers::UartDriver::~UartDriver() {
|
||||
close(fd);
|
||||
}
|
||||
55
lib/port/unix/udp.cpp
Normal file
55
lib/port/unix/udp.cpp
Normal file
@@ -0,0 +1,55 @@
|
||||
#include "port/udp.h"
|
||||
|
||||
#include <cstring>
|
||||
#include <poll.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <netinet/in.h>
|
||||
#include <sys/socket.h>
|
||||
|
||||
#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<const uint8_t> 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<uint8_t>& 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);
|
||||
}
|
||||
64
lib/port/win/poller.cpp
Normal file
64
lib/port/win/poller.cpp
Normal file
@@ -0,0 +1,64 @@
|
||||
#include "port/poller.h"
|
||||
#include <windows.h>
|
||||
#include <stdexcept>
|
||||
|
||||
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<HANDLE> 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<DWORD>(timeoutMs);
|
||||
DWORD rc = WaitForMultipleObjects(static_cast<DWORD>(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<int>(handles.size())) {
|
||||
auto& obj = objects[idx];
|
||||
obj->revents = 1; // простая метка, что событие произошло
|
||||
}
|
||||
}
|
||||
|
||||
poller::PollWrapper::~PollWrapper() = default;
|
||||
71
lib/port/win/uart.cpp
Normal file
71
lib/port/win/uart.cpp
Normal file
@@ -0,0 +1,71 @@
|
||||
#include "port/poller.h"
|
||||
#include "port/uart.h"
|
||||
#include <Windows.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
|
||||
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<const uint8_t> 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<uint8_t>& 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);
|
||||
}
|
||||
|
||||
66
lib/port/win/udp.cpp
Normal file
66
lib/port/win/udp.cpp
Normal file
@@ -0,0 +1,66 @@
|
||||
// File: src/port/win/udp_driver_win.cpp
|
||||
#include "pollobject.h"
|
||||
#include <winsock2.h>
|
||||
#include <ws2tcpip.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
|
||||
#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<const uint8_t> 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<uint8_t>& 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
|
||||
@@ -1,5 +0,0 @@
|
||||
{
|
||||
"dependencies": [
|
||||
"sdl2"
|
||||
]
|
||||
}
|
||||
Reference in New Issue
Block a user