исправление зависания программы спустя пол часа

air/main.cpp

@@ -1,4 +1,5 @@
 #include <sys/ioctl.h>
+#include <sys/poll.h>
 #include <asm/termbits.h>
 #include <sys/socket.h>
 #include <netinet/in.h>
@@ -11,17 +12,26 @@
 #include <cstring>
 #include <atomic>
 #include <span>
+#include <time.h>
 
+/**
+ * Вспомогательная функция для получения текущего времени в миллисекундах
+ */
+inline int64_t milliseconds() {
+    timespec ts{};
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    return static_cast<int64_t>(ts.tv_sec) * 1000 + static_cast<int64_t>(ts.tv_nsec) / 1000000;
+}
 
-
+constexpr int64_t SBUS_SEND_FRAME_INTERVAL = 50;
+constexpr int64_t SBUS_RXLOSS_INTERVAL = 500;
 
 class UDPServer {
-private:
+public:
     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);
@@ -84,7 +94,9 @@ struct SbusData {
     bool lost_frame = false;
     bool failsafe = false;
     bool ch17 = false, ch18 = false;
-    static constexpr size_t NUM_CH = 16;
+    static constexpr size_t NUM_CH = 18;
+    static constexpr int16_t SBUS_CH_MIN = 173 + 20;
+    static constexpr int16_t SBUS_CH_MAX = 1812 - 20;
     int16_t ch[NUM_CH];
 
     /* Message len */
@@ -98,58 +110,73 @@ struct SbusData {
     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 binaryBuffer[25];
-    uint8_t buf_[BUF_LEN_];
+
+    typedef struct sbusChannels_s {
+        // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
+        unsigned int chan0 : 11;
+        unsigned int chan1 : 11;
+        unsigned int chan2 : 11;
+        unsigned int chan3 : 11;
+        unsigned int chan4 : 11;
+        unsigned int chan5 : 11;
+        unsigned int chan6 : 11;
+        unsigned int chan7 : 11;
+        unsigned int chan8 : 11;
+        unsigned int chan9 : 11;
+        unsigned int chan10 : 11;
+        unsigned int chan11 : 11;
+        unsigned int chan12 : 11;
+        unsigned int chan13 : 11;
+        unsigned int chan14 : 11;
+        unsigned int chan15 : 11;
+    } __attribute__((__packed__)) sbusChannels_t;
+    static_assert(sizeof(sbusChannels_s) == 22);
+    struct sbusFrame_s {
+        uint8_t syncByte = HEADER_;
+        sbusChannels_t channels{};
+        uint8_t flags{};
+        /**
+         * The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the value indicates the telemetry byte that is sent after every 4th sbus frame.
+         *
+         * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
+         * and
+         * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
+         */
+        uint8_t endByte = FOOTER_;
+    } __attribute__ ((__packed__));
+    static_assert(sizeof(sbusFrame_s) == sizeof(binaryBuffer));
 
     void fillDataBuf() {
-        /* Assemble packet */
+        auto* dest = reinterpret_cast<sbusFrame_s*>(binaryBuffer);
-        buf_[0] = HEADER_;
+        dest->syncByte = HEADER_;
-        buf_[1] = static_cast<uint8_t>((ch[0] & 0x07FF));
+        dest->channels.chan0 = std::min(std::max(ch[0], SBUS_CH_MIN), SBUS_CH_MAX);
-        buf_[2] = static_cast<uint8_t>((ch[0] & 0x07FF) >> 8 |
+        dest->channels.chan1 = std::min(std::max(ch[1], SBUS_CH_MIN), SBUS_CH_MAX);
-                (ch[1] & 0x07FF) << 3);
+        dest->channels.chan2 = std::min(std::max(ch[2], SBUS_CH_MIN), SBUS_CH_MAX);
-        buf_[3] = static_cast<uint8_t>((ch[1] & 0x07FF) >> 5 |
+        dest->channels.chan3 = std::min(std::max(ch[3], SBUS_CH_MIN), SBUS_CH_MAX);
-                (ch[2] & 0x07FF) << 6);
+        dest->channels.chan4 = std::min(std::max(ch[4], SBUS_CH_MIN), SBUS_CH_MAX);
-        buf_[4] = static_cast<uint8_t>((ch[2] & 0x07FF) >> 2);
+        dest->channels.chan5 = std::min(std::max(ch[5], SBUS_CH_MIN), SBUS_CH_MAX);
-        buf_[5] = static_cast<uint8_t>((ch[2] & 0x07FF) >> 10 |
+        dest->channels.chan6 = std::min(std::max(ch[6], SBUS_CH_MIN), SBUS_CH_MAX);
-                (ch[3] & 0x07FF) << 1);
+        dest->channels.chan7 = std::min(std::max(ch[7], SBUS_CH_MIN), SBUS_CH_MAX);
-        buf_[6] = static_cast<uint8_t>((ch[3] & 0x07FF) >> 7 |
+        dest->channels.chan8 = std::min(std::max(ch[8], SBUS_CH_MIN), SBUS_CH_MAX);
-                (ch[4] & 0x07FF) << 4);
+        dest->channels.chan9 = std::min(std::max(ch[9], SBUS_CH_MIN), SBUS_CH_MAX);
-        buf_[7] = static_cast<uint8_t>((ch[4] & 0x07FF) >> 4 |
+        dest->channels.chan10 = std::min(std::max(ch[10], SBUS_CH_MIN), SBUS_CH_MAX);
-                (ch[5] & 0x07FF) << 7);
+        dest->channels.chan11 = std::min(std::max(ch[11], SBUS_CH_MIN), SBUS_CH_MAX);
-        buf_[8] = static_cast<uint8_t>((ch[5] & 0x07FF) >> 1);
+        dest->channels.chan12 = std::min(std::max(ch[12], SBUS_CH_MIN), SBUS_CH_MAX);
-        buf_[9] = static_cast<uint8_t>((ch[5] & 0x07FF) >> 9  |
+        dest->channels.chan13 = std::min(std::max(ch[13], SBUS_CH_MIN), SBUS_CH_MAX);
-                (ch[6] & 0x07FF) << 2);
+        dest->channels.chan14 = std::min(std::max(ch[14], SBUS_CH_MIN), SBUS_CH_MAX);
-        buf_[10] = static_cast<uint8_t>((ch[6] & 0x07FF) >> 6  |
+        dest->channels.chan15 = std::min(std::max(ch[15], SBUS_CH_MIN), SBUS_CH_MAX);
-                 (ch[7] & 0x07FF) << 5);
+
-        buf_[11] = static_cast<uint8_t>((ch[7] & 0x07FF) >> 3);
+        dest->flags = 0;
-        buf_[12] = static_cast<uint8_t>((ch[8] & 0x07FF));
+        if (ch[16] >= (SBUS_CH_MAX + SBUS_CH_MAX) / 2) { dest->flags |= CH17_MASK_; }
-        buf_[13] = static_cast<uint8_t>((ch[8] & 0x07FF) >> 8 |
+        if (ch[17] >= (SBUS_CH_MAX + SBUS_CH_MAX) / 2) { dest->flags |= CH18_MASK_; }
-                 (ch[9]  & 0x07FF) << 3);
+
-        buf_[14] = static_cast<uint8_t>((ch[9] & 0x07FF) >> 5 |
+        dest->endByte = FOOTER_;
-                 (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:
@@ -159,7 +186,7 @@ public:
         // Открытие последовательного порта
         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;
+            std::cout << "Failed to open serial port " << port_path << ": " << std::strerror(errno) << std::endl;
             return false;
         }
         struct termios2 tio{};
@@ -181,7 +208,7 @@ public:
         tio.c_cflag |= (BOTHER|CREAD|CLOCAL);
 
         if (ioctl(fd, TCSETS2, &tio) != 0) {
-            std::cerr << "Failed to set termios2 attributes: " << std::strerror(errno) << std::endl;
+            std::cout << "Failed to set termios2 attributes: " << std::strerror(errno) << std::endl;
             close(fd);
             return false;
         }
@@ -206,25 +233,37 @@ public:
         return fd;
     }
 
+    int drain() {
+        int ret;
+        do {
+            ret = ioctl(fd, TCSBRK, 1);
+        } while (ret < 0 && errno == EINTR);
+
+        return ret;
+    }
+
     // Метод для записи данных в порт
-    bool write(std::span<const uint8_t> data) {
+    bool writeBuffer(std::span<const uint8_t> data) {
         if (fd < 0) return false;
 
-        ssize_t written = ::write(fd, data.data(), data.size());
+        ssize_t written = write(fd, data.data(), data.size());
         if (written < 0) {
-            std::cerr << "Failed to write to serial port" << std::endl;
+            std::cout << "Failed to write to serial port " << strerror(errno) << std::endl;
             return false;
         }
 
         // Принудительная отправка данных
-        // tcdrain(fd);
+        if (drain() < 0) {
+            std::cout << "Failed to flush data to serial port" << strerror(errno) << std::endl;
+            return false;
+        }
         return true;
     }
 };
 
 int main(int argc, char* argv[]) {
     // Парсим аргументы командной строки
-    std::string serial_port = "/dev/ttyUSB0";
+    std::string serial_port = "/dev/ttyPS0";
     if (argc > 1) {
         serial_port = argv[1];
     }
@@ -243,36 +282,79 @@ int main(int argc, char* argv[]) {
         std::cout << "Press Ctrl+C to exit" << std::endl;
 
         SbusData sb{};
+        int64_t lastSbusWrite = 0;
+        int64_t lastSbusRecv = 0;
 
-        int packet_count = 0;
+        int packetCount = 0;
+        int totalPacketCount = 0;
+        int serialErrors = 0;
+        int64_t lastStatisticsShow = milliseconds();
+        int64_t _lastLoopNow = lastStatisticsShow;
         while (true) {
+            pollfd udpFd{.fd = udp_server.sockfd, .events = POLLIN, .revents = 0};
+            poll(&udpFd, 1, SBUS_SEND_FRAME_INTERVAL / 4);
+            auto now = milliseconds();
+            if (std::abs(now - _lastLoopNow) > SBUS_SEND_FRAME_INTERVAL) {
+                std::cout << "Warning: Loop freeze, reset timers. now time:" << now << std::endl;
+                lastSbusWrite = 0;
+                lastSbusRecv = 0;
+                lastStatisticsShow = 0;
+            }
+            _lastLoopNow = now;
+
+            if (udpFd.revents & POLLIN) {
                 // Прием UDP пакета
                 std::vector<uint16_t> data = udp_server.receive();
+                lastSbusRecv = now;
 
                 if (!data.empty()) {
-                packet_count++;
+                    packetCount++;
 
-                for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) {
+                    for (int i = 0; i < SbusData::NUM_CH; ++i) {
-                    auto item = static_cast<int16_t>(data[i]);
+                        auto item = i < data.size() ? static_cast<double>(data[i]) : 1500.0;
-                    sb.ch[i] = static_cast<int16_t>((item - 1000.0) * 2);
+                        item -= 1000.0;
-                    if (sb.ch[i] < 50) {
+                        item = std::min(item, 1000.0);
-                        sb.ch[i] = 50; // минимальное число
+                        item = std::max(item, 0.0);
-                    } else if (sb.ch[i] > 1900) {
+                        item *= (SbusData::SBUS_CH_MAX - SbusData::SBUS_CH_MIN) / 1000.0;
-                        sb.ch[i] = 1900; // максимальное число
+                        item += SbusData::SBUS_CH_MIN;
+
+                        sb.ch[i] = static_cast<int16_t>(item);
+                        if (sb.ch[i] < SbusData::SBUS_CH_MIN) {
+                            sb.ch[i] = SbusData::SBUS_CH_MIN; // минимальное число
+                        } else if (sb.ch[i] > SbusData::SBUS_CH_MAX) {
+                            sb.ch[i] = SbusData::SBUS_CH_MAX; // максимальное число
                         }
                     }
                     sb.fillDataBuf();
-                serial.write(sb.buf_);
-
-                // Выводим статистику каждые 100 пакетов
-                if (packet_count % 100 == 0) {
-                    std::cout << "Received " << packet_count << " packets total" << std::endl;
                 }
             }
+
+            // std::cout << "from loop: current time " << now << std::endl;
+            if (now - lastSbusWrite >= SBUS_SEND_FRAME_INTERVAL && now - lastSbusRecv <= SBUS_RXLOSS_INTERVAL) {
+                lastSbusWrite = now;
+                if (!serial.writeBuffer(sb.binaryBuffer)) {
+                    serialErrors++;
+                } else {
+                    serialErrors = 0;
+                }
+
+                // после 50 ошибок дальше не будем пытаться что-то писать, ибо это бесполезно
+                if (serialErrors >= 50) {
+                    std::cout << "FATAL: 50 errors on serial port write operation, exit" << std::endl;
+                    break;
+                }
+            }
+
+            if (now - lastStatisticsShow >= 1000) {
+                lastStatisticsShow = now;
+                totalPacketCount += packetCount;
+                std::cout << "Received " << totalPacketCount << " packets total, " << packetCount << " in last second" << std::endl;
+                packetCount = 0;
+            }
         }
 
     } catch (const std::exception& e) {
-        std::cerr << "Error: " << e.what() << std::endl;
+        std::cout << "Error: " << e.what() << std::endl;
         return 1;
     }
 

ground/joystick-reader.cpp

@@ -64,13 +64,12 @@ bool JoystickReader::readData(std::vector<uint16_t>& data) {
     // Читаем кнопки
     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] = SDL_JoystickGetButton(joystick, i) ? 2000 : 1000;
-        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 < 1000) i = 1000;
-        if (i > 2050) i = 2050;
+        if (i > 2000) i = 2000;
     }
     return true;
 }

ground/main.cpp

@@ -27,8 +27,8 @@ int main(int argc, char* argv[]) {
 
     JoystickReader reader;
     const int64_t timeInterval = 1000 / frequency;
-    while (!reader.initialize()) {
+    if (!reader.initialize()) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        return 1;
     }
 
     UDPSocket udp(sendAddress, 1066);