More fixes and improvements (#57)

vertexodessa · web-flow · commit 432e3cef7e37 · 2025-04-25T00:22:44.000+02:00
diff --git a/app/mavlink/src/main/cpp/mavlink.cpp b/app/mavlink/src/main/cpp/mavlink.cpp
@@ -96,7 +96,14 @@ void *listen(int mavlink_port) {
         memset(buffer, 0x00, sizeof(buffer));
         int ret = recv(fd, buffer, sizeof(buffer), 0);
         if (ret < 0) {
-            continue;
+            // Check for timeout vs real error
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                __android_log_print(ANDROID_LOG_DEBUG, TAG, "Mavlink recv timeout");
+                continue;
+            } else {
+                __android_log_print(ANDROID_LOG_ERROR, TAG, "Error receiving mavlink: %s", strerror(errno));
+                return 0;
+            }
         } else if (ret == 0) {
             // peer has done an orderly shutdown
             __android_log_print(ANDROID_LOG_ERROR, TAG, "Shutting down mavlink: ret=0");
diff --git a/app/videonative/src/main/cpp/BufferedPacketQueue.h b/app/videonative/src/main/cpp/BufferedPacketQueue.h
@@ -16,9 +16,9 @@
 // Define logging tag and maximum buffer size
 #define BUFFERED_QUEUE_LOG_TAG "BufferedPacketQueue"
 // Considering the packet rate about 100 packets per second, 10 packets should be enough
-constexpr size_t MAX_BUFFER_SIZE = 5;
+constexpr size_t MAX_BUFFER_SIZE = 15;
 // Number of monotonically increasing packets
-constexpr size_t MONOTONIC_THRESHOLD = 3;
+constexpr size_t MONOTONIC_THRESHOLD = 5;
 
 // Type definition for sequence numbers
 using SeqType   = uint16_t;
diff --git a/app/videonative/src/main/cpp/parser/ParseRTP.cpp b/app/videonative/src/main/cpp/parser/ParseRTP.cpp
@@ -123,6 +123,13 @@ void RTPDecoder::parseRTPH264toNALU(const uint8_t* rtp_data, const size_t data_l
     }
     // MLOGD<<"Got h264 rtp data";
     const RTP::RTPPacketH264 rtpPacket(rtp_data, data_length);
+
+    if (rtpPacket.rtpPayloadSize == 0)
+    {
+        MLOGD << "RTP packet is empty";
+        return;
+    }
+
     // MLOGD<<"RTP Header: "<<rtp_header->asString();
     if (!validateRTPPacket(rtpPacket.header))
     {
diff --git a/app/videonative/src/main/cpp/parser/RTP.hpp b/app/videonative/src/main/cpp/parser/RTP.hpp
@@ -205,7 +205,7 @@ class RTPPacket
         //  r.n we don't support padding
         // assert(header.padding == 0);
 
-        if(header.padding >0 )
+        if (header.padding > 0)
         {
             rtpPayloadSize = 0;
         }
diff --git a/app/wfbngrtl8812/src/main/cpp/TxFrame.cpp b/app/wfbngrtl8812/src/main/cpp/TxFrame.cpp
@@ -492,13 +492,69 @@ uint32_t TxFrame::extractRxqOverflow(struct msghdr *msg) {
     return 0;
 }
 
+int TxFrame::open_udp_socket_for_rx(int port, int buf_size) {
+    int fd = ::socket(AF_INET, SOCK_DGRAM, 0);
+    if (fd < 0) {
+        throw std::runtime_error(string_format("Unable to open socket: %s", std::strerror(errno)));
+    }
+
+    int optval = 1;
+    setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
+
+    // Set receive timeout to 500ms
+    struct timeval tv;
+    tv.tv_sec = 0;
+    tv.tv_usec = 500000; // 500ms
+    if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) {
+        ::close(fd);
+        throw std::runtime_error(string_format("Unable to set socket timeout: %s", std::strerror(errno)));
+    }
+
+    if (buf_size) {
+        if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &buf_size, sizeof(buf_size)) < 0) {
+            ::close(fd);
+            throw std::runtime_error(string_format("Unable to set requested buffer size: %s", std::strerror(errno)));
+        }
+        int actual_buf_size = 0;
+        socklen_t optlen = sizeof(actual_buf_size);
+        getsockopt(fd, SOL_SOCKET, SO_RCVBUF, &actual_buf_size, &optlen);
+        if (actual_buf_size < buf_size * 2) {
+            // Linux doubles the value we set
+            fprintf(stderr, "Warning: requested rx buffer size %d but got %d\n", buf_size, actual_buf_size / 2);
+        }
+    }
+
+    struct sockaddr_in saddr;
+    std::memset(&saddr, 0, sizeof(saddr));
+    saddr.sin_family = AF_INET;
+    saddr.sin_addr.s_addr = htonl(INADDR_ANY);
+    saddr.sin_port = htons(static_cast<uint16_t>(port));
+
+    if (::bind(fd, reinterpret_cast<struct sockaddr *>(&saddr), sizeof(saddr)) < 0) {
+        ::close(fd);
+        throw std::runtime_error(string_format("Unable to bind to port %d: %s", port, std::strerror(errno)));
+    }
+
+    return fd;
+}
+
 void TxFrame::dataSource(
     std::shared_ptr<Transmitter> &transmitter, std::vector<int> &rxFds, int fecTimeout, bool mirror, int logInterval) {
     int nfds = static_cast<int>(rxFds.size());
     if (nfds <= 0) {
         throw std::runtime_error("dataSource: no valid rx sockets");
     }
 
+    // Set timeout on all sockets
+    for (int fd : rxFds) {
+        struct timeval tv;
+        tv.tv_sec = 0;
+        tv.tv_usec = 500000; // 500ms
+        if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) {
+            throw std::runtime_error(string_format("Unable to set socket timeout: %s", std::strerror(errno)));
+        }
+    }
+
     std::vector<pollfd> fds(nfds);
     for (int i = 0; i < nfds; ++i) {
         fds[i].fd = rxFds[i];
@@ -650,7 +706,8 @@ void TxFrame::dataSource(
 
                     ssize_t rsize = ::recvmsg(pfd.fd, &msg, 0);
                     if (rsize < 0) {
-                        if (errno != EWOULDBLOCK && errno != EAGAIN) {
+                        if (errno != EWOULDBLOCK && errno != EAGAIN && errno != ETIMEDOUT) {
+                            continue;
                             throw std::runtime_error(string_format("Error receiving packet: %s", std::strerror(errno)));
                         }
                         break;
@@ -809,7 +866,7 @@ void TxFrame::run(Rtl8812aDevice *rtlDevice, TxArgs *arg) {
     // Attempt to create a UDP listening socket
     std::vector<int> rxFds;
     int bindPort = arg->udp_port;
-    int udpFd = open_udp_socket_for_rx(bindPort, arg->rcv_buf);
+    int udpFd = TxFrame::open_udp_socket_for_rx(bindPort, arg->rcv_buf);
 
     if (arg->udp_port == 0) {
         // ephemeral port
diff --git a/app/wfbngrtl8812/src/main/cpp/TxFrame.h b/app/wfbngrtl8812/src/main/cpp/TxFrame.h
@@ -102,7 +102,7 @@ class Transmitter {
 
     /**
      * @brief Choose which output interface (antenna / socket / etc.) to use.
-     * @param idx The interface index, or -1 for “mirror” mode.
+     * @param idx The interface index, or -1 for "mirror" mode.
      */
     virtual void selectOutput(int idx) = 0;
 
@@ -206,7 +206,7 @@ using TxAntennaStat = std::unordered_map<uint64_t, TxAntennaItem>;
  * @class RawSocketTransmitter
  * @brief Transmitter that sends packets over raw AF_PACKET sockets.
  *
- * This transmitter can operate in single-output or “mirror” mode. In mirror mode,
+ * This transmitter can operate in single-output or "mirror" mode. In mirror mode,
  * it sends each packet through all raw sockets. In single-output mode, only one is selected.
  */
 class RawSocketTransmitter : public Transmitter {
@@ -391,4 +391,12 @@ class TxFrame {
 
   private:
     bool shouldStop_ = false;
+
+    /**
+     * @brief Create a UDP socket for receiving data
+     * @param port UDP port to bind to
+     * @param buf_size Receive buffer size
+     * @return Socket file descriptor
+     */
+    static int open_udp_socket_for_rx(int port, int buf_size);
 };
diff --git a/app/wfbngrtl8812/src/main/cpp/WfbngLink.cpp b/app/wfbngrtl8812/src/main/cpp/WfbngLink.cpp
@@ -58,7 +58,6 @@ void WfbngLink::initAgg() {
     std::string client_addr = "127.0.0.1";
     uint64_t epoch = 0;
 
-    int video_client_port = 5600;
     uint8_t video_radio_port = 0;
     uint32_t video_channel_id_f = (link_id << 8) + video_radio_port;
     video_channel_id_be = htobe32(video_channel_id_f);
@@ -79,13 +78,14 @@ void WfbngLink::initAgg() {
     uint32_t udp_channel_id_f = (link_id << 8) + udp_radio_port;
     udp_channel_id_be = htobe32(udp_channel_id_f);
 
-    udp_aggregator = std::make_unique<AggregatorUDPv4>(client_addr, udp_client_port, keyPath, epoch, udp_channel_id_f, 0);
+    udp_aggregator =
+        std::make_unique<AggregatorUDPv4>(client_addr, udp_client_port, keyPath, epoch, udp_channel_id_f, 0);
 }
 
 int WfbngLink::run(JNIEnv *env, jobject context, jint wifiChannel, jint bw, jint fd) {
     int r;
     libusb_context *ctx = NULL;
-    std::shared_ptr<TxFrame> txFrame = std::make_shared<TxFrame>();
+    txFrame = std::make_shared<TxFrame>();
 
     r = libusb_set_option(NULL, LIBUSB_OPTION_NO_DEVICE_DISCOVERY);
     r = libusb_init(&ctx);
@@ -214,7 +214,8 @@ int WfbngLink::run(JNIEnv *env, jobject context, jint wifiChannel, jint bw, jint
             Rtl8812aDevice *current_device = rtl_devices.at(fd).get();
             if (!usb_tx_thread) {
                 init_thread(usb_tx_thread, [&]() {
-                    return std::make_unique<std::thread>([txFrame, current_device, args] {
+                    return std::make_unique<std::thread>([this, current_device, args] {
+                        setName(pthread_self(), "usb_transfer");
                         txFrame->run(current_device, args.get());
                         __android_log_print(ANDROID_LOG_DEBUG, TAG, "usb_transfer thread should terminate");
                     });
diff --git a/app/wfbngrtl8812/src/main/cpp/WfbngLink.hpp b/app/wfbngrtl8812/src/main/cpp/WfbngLink.hpp
@@ -1,6 +1,7 @@
 #ifndef FPV_VR_WFBNG_LINK_H
 #define FPV_VR_WFBNG_LINK_H
 
+#include "TxFrame.h"
 extern "C" {
 #include "wfb-ng/src/fec.h"
 }
@@ -67,9 +68,18 @@ class WfbngLink {
     }
 
   private:
+    void stopDevice() {
+        if (rtl_devices.find(current_fd) == rtl_devices.end()) return;
+        auto dev = rtl_devices.at(current_fd).get();
+        if (dev) {
+            dev->should_stop = true;
+        }
+    }
+
     const char *keyPath = "/data/user/0/com.openipc.pixelpilot/files/gs.key";
     std::recursive_mutex thread_mutex;
     std::unique_ptr<WiFiDriver> wifi_driver;
+    std::shared_ptr<TxFrame> txFrame;
     uint32_t video_channel_id_be;
     uint32_t mavlink_channel_id_be;
     uint32_t udp_channel_id_be;

Original file line number	Diff line number	Diff line change
`@@ -123,6 +123,13 @@ void RTPDecoder::parseRTPH264toNALU(const uint8_t* rtp_data, const size_t data_l`
`123`	`123`	`}`
`124`	`124`	`// MLOGD<<"Got h264 rtp data";`
`125`	`125`	`const RTP::RTPPacketH264 rtpPacket(rtp_data, data_length);`
	`126`	`+`
	`127`	`+ if (rtpPacket.rtpPayloadSize == 0)`
	`128`	`+ {`
	`129`	`+ MLOGD << "RTP packet is empty";`
	`130`	`+ return;`
	`131`	`+ }`
	`132`	`+`
`126`	`133`	`// MLOGD<<"RTP Header: "<<rtp_header->asString();`
`127`	`134`	`if (!validateRTPPacket(rtpPacket.header))`
`128`	`135`	`{`
Original file line number	Diff line number	Diff line change
`@@ -205,7 +205,7 @@ class RTPPacket`
`205`	`205`	`// r.n we don't support padding`
`206`	`206`	`// assert(header.padding == 0);`
`207`	`207`
`208`		`- if(header.padding >0 )`
	`208`	`+ if (header.padding > 0)`
`209`	`209`	`{`
`210`	`210`	`rtpPayloadSize = 0;`
`211`	`211`	`}`