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

air/main.cpp

@@ -12,18 +12,18 @@
 #include <cstring>
 #include <atomic>
 #include <span>
-#include <sys/time.h>
+#include <time.h>
 
 /**
  * Вспомогательная функция для получения текущего времени в миллисекундах
  */
 inline int64_t milliseconds() {
-    timeval tv{};
-    gettimeofday(&tv,nullptr);
-    return ((tv.tv_sec * 1000000l) + tv.tv_usec) / 1000;
+    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 = 15;
+constexpr int64_t SBUS_SEND_FRAME_INTERVAL = 50;
 constexpr int64_t SBUS_RXLOSS_INTERVAL = 500;
 
 class UDPServer {
@@ -95,8 +95,8 @@ struct SbusData {
     bool failsafe = false;
     bool ch17 = false, ch18 = false;
     static constexpr size_t NUM_CH = 18;
-    static constexpr int16_t SBUS_CH_MIN = 173 + 10;
-    static constexpr int16_t SBUS_CH_MAX = 1812 - 10;
+    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 */
@@ -112,42 +112,42 @@ struct SbusData {
     static constexpr uint8_t FAILSAFE_MASK_ = 0x08;
     uint8_t binaryBuffer[25];
 
-    void fillDataBuf() {
-        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));
+    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() {
         auto* dest = reinterpret_cast<sbusFrame_s*>(binaryBuffer);
         dest->syncByte = HEADER_;
         dest->channels.chan0 = std::min(std::max(ch[0], SBUS_CH_MIN), SBUS_CH_MAX);
@@ -169,7 +169,7 @@ struct SbusData {
 
         dest->flags = 0;
         if (ch[16] >= (SBUS_CH_MAX + SBUS_CH_MAX) / 2) { dest->flags |= CH17_MASK_; }
-        if (ch[16] >= (SBUS_CH_MAX + SBUS_CH_MAX) / 2) { dest->flags |= CH18_MASK_; }
+        if (ch[17] >= (SBUS_CH_MAX + SBUS_CH_MAX) / 2) { dest->flags |= CH18_MASK_; }
 
         dest->endByte = FOOTER_;
     }
@@ -177,7 +177,6 @@ struct SbusData {
 };
 
 class SerialPort {
-private:
     int fd;
 
 public:
@@ -187,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{};
@@ -209,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;
         }
@@ -234,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];
     }
@@ -276,21 +287,31 @@ int main(int argc, char* argv[]) {
 
         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 = milliseconds();
+                lastSbusRecv = now;
 
                 if (!data.empty()) {
                     packetCount++;
 
-                    for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) {
-                        auto item = static_cast<double>(data[i]);
+                    for (int i = 0; i < SbusData::NUM_CH; ++i) {
+                        auto item = i < data.size() ? static_cast<double>(data[i]) : 1500.0;
                         item -= 1000.0;
                         item = std::min(item, 1000.0);
                         item = std::max(item, 0.0);
@@ -308,10 +329,18 @@ int main(int argc, char* argv[]) {
                 }
             }
 
-            const auto now = milliseconds();
+            // 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.write(sb.binaryBuffer)) {
+                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;
                 }
             }
@@ -325,7 +354,7 @@ int main(int argc, char* argv[]) {
         }
 
     } catch (const std::exception& e) {
-        std::cerr << "Error: " << e.what() << std::endl;
+        std::cout << "Error: " << e.what() << std::endl;
         return 1;
     }