diff --git a/air/main.cpp b/air/main.cpp index a3bbcb5..bd6c48b 100644 --- a/air/main.cpp +++ b/air/main.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -11,17 +12,26 @@ #include #include #include +#include +/** + * Вспомогательная функция для получения текущего времени в миллисекундах + */ +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 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 data = udp_server.receive(); + lastSbusRecv = milliseconds(); - for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) { - auto item = static_cast(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(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(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(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; } }