попытка фикса RXLOSS для sbus

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 <sys/time.h>
 
+/**
+ * Вспомогательная функция для получения текущего времени в миллисекундах
+ */
+inline int64_t milliseconds() {
+    timeval tv{};
+    gettimeofday(&tv,nullptr);
+    return ((tv.tv_sec * 1000000l) + tv.tv_usec) / 1000;
+}
 
-
+constexpr int64_t SBUS_SEND_FRAME_INTERVAL = 25;
+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);
@@ -245,39 +255,56 @@ 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;
+        int64_t lastStatisticsShow = milliseconds();
         while (true) {
-            // Прием UDP пакета
-            std::vector<uint16_t> data = udp_server.receive();
+            pollfd udpFd{.fd = udp_server.sockfd, .events = POLLIN, .revents = 0};
+            poll(&udpFd, 1, SBUS_SEND_FRAME_INTERVAL / 4);
 
-            if (!data.empty()) {
-                packet_count++;
+            if (udpFd.revents & POLLIN) {
+                // Прием UDP пакета
+                std::vector<uint16_t> data = udp_server.receive();
+                lastSbusRecv = milliseconds();
 
-                for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) {
-                    auto item = static_cast<double>(data[i]);
-                    item -= 1000.0;
-                    item = std::min(item, 1000.0);
-                    item = std::max(item, 0.0);
-                    item *= (SbusData::SBUS_CH_MAX - SbusData::SBUS_CH_MIN) / 1000.0;
-                    item += SbusData::SBUS_CH_MIN;
+                if (!data.empty()) {
+                    packetCount++;
 
-                    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; // максимальное число
+                    for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) {
+                        auto item = static_cast<double>(data[i]);
+                        item -= 1000.0;
+                        item = std::min(item, 1000.0);
+                        item = std::max(item, 0.0);
+                        item *= (SbusData::SBUS_CH_MAX - SbusData::SBUS_CH_MIN) / 1000.0;
+                        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();
                 }
-                sb.fillDataBuf();
+            }
+
+            const auto now = milliseconds();
+            if (now - lastSbusWrite >= SBUS_SEND_FRAME_INTERVAL && now - lastSbusRecv <= SBUS_RXLOSS_INTERVAL) {
+                lastSbusWrite = now;
                 if (!serial.write(sb.buf_)) {
                     break;
                 }
+            }
 
-                // Выводим статистику каждые 100 пакетов
-                if (packet_count % 100 == 0) {
-                    std::cout << "Received " << packet_count << " packets total" << std::endl;
-                }
+            if (now - lastStatisticsShow >= 1000) {
+                lastStatisticsShow = now;
+                totalPacketCount += packetCount;
+                std::cout << "Received " << totalPacketCount << " packets total, " << packetCount << " in last second" << std::endl;
+                packetCount = 0;
             }
         }