Skip to content

Commit

Permalink
Remove SocketAPM and use new SocketUDP
Browse files Browse the repository at this point in the history
  • Loading branch information
khancyr authored and srmainwaring committed Oct 30, 2024
1 parent 1ea21c1 commit 2668eb7
Show file tree
Hide file tree
Showing 6 changed files with 224 additions and 322 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
73 changes: 0 additions & 73 deletions include/Socket.h

This file was deleted.

75 changes: 75 additions & 0 deletions include/SocketUDP.hh
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@
/*
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);
};
#endif // SOCKETUDP_HH_
12 changes: 4 additions & 8 deletions src/ArduPilotPlugin.cc
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@

#include <sdf/sdf.hh>

#include "Socket.h"
#include "SocketUDP.hh"
#include "Util.hh"

#define DEBUG_JSON_IO 0
Expand Down Expand Up @@ -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"};
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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,
Expand All @@ -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;
Expand Down
Loading

0 comments on commit 2668eb7

Please sign in to comment.