Remove SocketAPM and use new SocketUDP

CMakeLists.txt

@@ -64,7 +64,7 @@ pkg_check_modules(GST REQUIRED gstreamer-1.0 gstreamer-app-1.0)
 add_library(ArduPilotPlugin
     SHARED
     src/ArduPilotPlugin.cc
-    src/Socket.cpp
+    src/SocketUDP.cc
     src/Util.cc
 )
 target_include_directories(ArduPilotPlugin PRIVATE

include/SocketUDP.hh

@@ -0,0 +1,74 @@
+/*
+   Copyright (C) 2024 ardupilot.org
+
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU Lesser General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU Lesser General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef SOCKETUDP_HH_
+#define SOCKETUDP_HH_
+
+#include <fcntl.h>
+#include <unistd.h>
+#ifdef _WIN32
+    #include <winsock2.h>
+    #include <Ws2tcpip.h>
+#else
+    #include <sys/ioctl.h>
+    #include <sys/socket.h>
+    #include <netinet/in.h>
+    #include <netinet/tcp.h>
+    #include <arpa/inet.h>
+    #include <sys/select.h>
+#endif
+
+/// \brief Simple UDP socket handling class.
+class SocketUDP {
+public:
+    /// \brief Constructor.
+    SocketUDP(bool reuseaddress, bool blocking);
+
+    /// \brief Destructor.
+    ~SocketUDP();
+
+    /// \brief Bind socket to address and port.
+    bool bind(const char *address, uint16_t port);
+
+    /// \brief Set reuse address option.
+    bool set_reuseaddress();
+
+    /// \brief Set blocking state.
+    bool set_blocking(bool blocking);
+
+    /// \brief Send data to address and port.
+    ssize_t sendto(const void *buf, size_t size, const char *address, uint16_t port);
+
+    /// \brief Receive data.
+    ssize_t recv(void *pkt, size_t size, uint32_t timeout_ms);
+
+    /// \brief Get last client address and port
+    void get_client_address(const char *&ip_addr, uint16_t &port);
+
+private:
+    /// \brief File descriptor.
+    struct sockaddr_in in_addr {};
+
+    /// \brief File descriptor.
+    int fd = -1;
+
+    /// \brief Poll for incoming data with timeout.
+    bool pollin(uint32_t timeout_ms);
+
+    /// \brief Make a sockaddr_in struct from address and port.
+    void make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr);
+};

src/ArduPilotPlugin.cc

@@ -57,7 +57,7 @@
 
 #include <sdf/sdf.hh>
 
-#include "Socket.h"
+#include "SocketUDP.hh"
 #include "Util.hh"
 
 #define DEBUG_JSON_IO 0
@@ -219,7 +219,7 @@ class gz::sim::systems::ArduPilotPluginPrivate
   public: std::mutex mutex;
 
   /// \brief Socket manager
-  public: SocketAPM sock = SocketAPM(true);
+  public: SocketUDP sock = SocketUDP(true, true);
 
   /// \brief The address for the flight dynamics model (i.e. this plugin)
   public: std::string fdm_address{"127.0.0.1"};
@@ -1253,10 +1253,6 @@ void gz::sim::systems::ArduPilotPlugin::ResetPIDs()
 /////////////////////////////////////////////////
 bool gz::sim::systems::ArduPilotPlugin::InitSockets(sdf::ElementPtr _sdf) const
 {
-    // configure port
-    this->dataPtr->sock.set_blocking(false);
-    this->dataPtr->sock.reuseaddress();
-
     // get the fdm address if provided, otherwise default to localhost
     this->dataPtr->fdm_address =
         _sdf->Get("fdm_addr", static_cast<std::string>("127.0.0.1")).first;
@@ -1407,7 +1403,7 @@ namespace
 /// \brief Get a servo packet. Templated for 16 or 32 channel packets.
 template<typename TServoPacket>
 ssize_t getServoPacket(
-  SocketAPM &_sock,
+  SocketUDP &_sock,
   const char *&_fcu_address,
   uint16_t &_fcu_port_out,
   uint32_t _waitMs,
@@ -1417,7 +1413,7 @@ ssize_t getServoPacket(
 {
     ssize_t recvSize = _sock.recv(&_pkt, sizeof(TServoPacket), _waitMs);
 
-    _sock.last_recv_address(_fcu_address, _fcu_port_out);
+    _sock.get_client_address(_fcu_address, _fcu_port_out);
 
     // drain the socket in the case we're backed up
     int counter = 0;