From 8b7c7b71bc25ef3825a52f715b490cf67b13d000 Mon Sep 17 00:00:00 2001 From: Vladislav Ostapov Date: Fri, 14 Nov 2025 13:31:55 +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=B2=D1=81=D0=B5=20=D0=B2=D1=80=D0=BE=D0=B4=D0=B5=20=D1=80?= =?UTF-8?q?=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 --- air/main.cpp | 91 ++++++++++++++++---------------------- ground/joystick-reader.cpp | 23 +++++----- ground/joystick-reader.h | 3 +- ground/main.cpp | 19 +++++++- 4 files changed, 69 insertions(+), 67 deletions(-) diff --git a/air/main.cpp b/air/main.cpp index 9c00238..f2aead8 100644 --- a/air/main.cpp +++ b/air/main.cpp @@ -1,5 +1,5 @@ -#include #include +#include #include #include #include @@ -51,7 +51,7 @@ public: } std::vector receive() { - std::vector data(16); + std::vector data(64); ssize_t received = recvfrom(sockfd, data.data(), @@ -61,22 +61,19 @@ public: &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; + // Выводим информацию о отправителе и данные + 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; + // 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 data; } return {}; @@ -165,43 +162,30 @@ public: 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); - 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) { + if (ioctl(fd, TCSETS2, &tio) != 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; } @@ -268,8 +252,14 @@ int main(int argc, char* argv[]) { if (!data.empty()) { packet_count++; - for (int i = 0; i < data.size() && i < sb.NUM_CH; ++i) { - sb.ch[i] = static_cast(data[i]); + for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) { + auto item = static_cast(data[i]); + sb.ch[i] = static_cast((item - 1000.0) * 2); + if (sb.ch[i] < 50) { + sb.ch[i] = 50; // минимальное число + } else if (sb.ch[i] > 1900) { + sb.ch[i] = 1900; // максимальное число + } } sb.fillDataBuf(); serial.write(sb.buf_); @@ -279,9 +269,6 @@ int main(int argc, char* argv[]) { std::cout << "Received " << packet_count << " packets total" << std::endl; } } - - // Небольшая пауза чтобы не грузить CPU - usleep(1000); // 1 мс } } catch (const std::exception& e) { diff --git a/ground/joystick-reader.cpp b/ground/joystick-reader.cpp index 5039d3a..cbac9be 100644 --- a/ground/joystick-reader.cpp +++ b/ground/joystick-reader.cpp @@ -3,8 +3,8 @@ #include #include -JoystickReader::JoystickReader(int frequency) - : joystick(nullptr), updateIntervalMs(1000 / frequency) {} +JoystickReader::JoystickReader() + : joystick(nullptr) {} JoystickReader::~JoystickReader() { if (joystick) { @@ -50,29 +50,28 @@ bool JoystickReader::readData(std::vector& data) { SDL_JoystickUpdate(); - data.resize(16, 1500); // Заполняем нейтральным значением + data.resize(24); // Заполняем нейтральным значением + for (auto& i: data) { i = 1500; } - // Читаем оси (обычно до 6 осей) - int axes = std::min(SDL_JoystickNumAxes(joystick), 6); - for (int i = 0; i < axes; ++i) { + 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) { - Uint8 buttonState = SDL_JoystickGetButton(joystick, i); - data[axes + i] = buttonState ? 2000 : 1000; + auto buttonState = SDL_JoystickGetButton(joystick, i); + data[axes + i] = static_cast(1000.0 + (buttonState * (1000.0 / 255.0))); } for (auto& i: data) { - if (i < 900) i = 900; - if (i > 2100) i = 2100; + if (i < 950) i = 950; + if (i > 2050) i = 2050; } - - std::this_thread::sleep_for(std::chrono::milliseconds(updateIntervalMs)); return true; } diff --git a/ground/joystick-reader.h b/ground/joystick-reader.h index 45df640..ced565a 100644 --- a/ground/joystick-reader.h +++ b/ground/joystick-reader.h @@ -13,7 +13,7 @@ class JoystickReader { public: - JoystickReader(int frequency = 50); + JoystickReader(); ~JoystickReader(); bool initialize(); @@ -22,7 +22,6 @@ public: private: SDL_Joystick* joystick; - int updateIntervalMs; std::string joystickName; }; diff --git a/ground/main.cpp b/ground/main.cpp index 4990e59..73bc784 100644 --- a/ground/main.cpp +++ b/ground/main.cpp @@ -6,6 +6,12 @@ #include "joystick-reader.h" #include "udp-driver.h" +int64_t milliseconds() { + timeval tv{}; + gettimeofday(&tv,nullptr); + return ((tv.tv_sec * 1000000l) + tv.tv_usec) / 1000; +} + int main(int argc, char* argv[]) { // Парсим аргументы командной строки для частоты @@ -25,7 +31,8 @@ int main(int argc, char* argv[]) { std::cout << "Starting joystick reader with frequency: " << frequency << " Hz" << std::endl; - JoystickReader reader(frequency); + JoystickReader reader; + const int64_t timeInterval = 1000 / frequency; while (!reader.initialize()) { std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } @@ -36,6 +43,7 @@ int main(int argc, char* argv[]) { std::cout << "Sending data to " << sendAddress << ":1066" << std::endl; int counter = 0; + auto now = milliseconds(); while (reader.readData(data)) { if (!udp.sendFrame(data)) { std::cerr << "Failed to send UDP packet" << std::endl; @@ -45,6 +53,15 @@ int main(int argc, char* argv[]) { if (++counter % 100 == 0) { std::cout << "Sent " << counter << " packets" << std::endl; } + + // вычислим время для сна + // должно быть время now+timeInterval + auto targetTime = now + timeInterval; + auto nowTime = milliseconds(); + if (targetTime > nowTime) { + std::this_thread::sleep_for(std::chrono::milliseconds(targetTime - nowTime)); + } + now = milliseconds(); } std::cout << "Exiting..." << std::endl;