Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enable SocketAPM to run on LwIP and add DDS support #25523

Merged
merged 13 commits into from
Nov 15, 2023
Merged
Show file tree
Hide file tree
Changes from 11 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions Tools/ardupilotwaf/boards.py
Original file line number Diff line number Diff line change
Expand Up @@ -148,6 +148,12 @@ def srcpath(path):
)
cfg.msg("Enabled custom controller", 'no', color='YELLOW')

if cfg.options.enable_networking:
env.CXXFLAGS += ['-DAP_NETWORKING_ENABLED=1']

if cfg.options.enable_networking_tests:
env.CXXFLAGS += ['-DAP_NETWORKING_TESTS_ENABLED=1']

d = env.get_merged_dict()
# Always prepend so that arguments passed in the command line get
# the priority.
Expand Down
10 changes: 10 additions & 0 deletions Tools/autotest/sim_vehicle.py
Original file line number Diff line number Diff line change
Expand Up @@ -411,6 +411,12 @@ def do_build(opts, frame_options):
if opts.enable_dds:
cmd_configure.append("--enable-dds")

if opts.enable_networking:
cmd_configure.append("--enable-networking")

if opts.enable_networking_tests:
cmd_configure.append("--enable-networking-tests")

pieces = [shlex.split(x) for x in opts.waf_configure_args]
for piece in pieces:
cmd_configure.extend(piece)
Expand Down Expand Up @@ -1326,6 +1332,10 @@ def generate_frame_help():
help="IP address of the simulator. Defaults to localhost")
group_sim.add_option("--enable-dds", action='store_true',
help="Enable the dds client to connect with ROS2/DDS")
group_sim.add_option("--enable-networking", action='store_true',
help="Enable networking")
group_sim.add_option("--enable-networking-tests", action='store_true',
help="Enable networking tests")

parser.add_option_group(group_sim)

Expand Down
4 changes: 4 additions & 0 deletions libraries/AP_DDS/AP_DDS_Client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,10 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] {
// @User: Standard
AP_GROUPINFO("_PORT", 2, AP_DDS_Client, udp.port, 2019),

// @Group: _IP
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there a way to add a description
"IP address of the DDS Agent"

Copy link
Contributor Author

@magicrub magicrub Nov 13, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think so.

Note, the _POPT docs are broken. Docs say "DDS_UDP_PORT" but code is "DDS_PORT". I'm in favor of removing the "UDP" part and have a different param control TCP/UDP server/client options

// @Path: ../AP_Networking/AP_Networking_address.cpp
AP_SUBGROUPINFO(udp.ip, "_IP", 3, AP_DDS_Client, AP_Networking_IPV4),

#endif

AP_GROUPEND
Expand Down
3 changes: 2 additions & 1 deletion libraries/AP_DDS/AP_DDS_Client.h
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@

#if AP_DDS_UDP_ENABLED
#include <AP_HAL/utility/Socket.h>
#include <AP_Networking/AP_Networking_address.h>
#endif

extern const AP_HAL::HAL& hal;
Expand Down Expand Up @@ -138,7 +139,7 @@ class AP_DDS_Client
struct {
AP_Int32 port;
// UDP endpoint
const char* ip = "127.0.0.1";
AP_Networking_IPV4 ip{AP_DDS_DEFAULT_UDP_IP_ADDR};
// UDP Allocation
uxrCustomTransport transport;
SocketAPM *socket;
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_DDS/AP_DDS_UDP.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ bool AP_DDS_Client::udp_transport_open(uxrCustomTransport *t)
if (sock == nullptr) {
return false;
}
if (!sock->connect(dds->udp.ip, dds->udp.port.get())) {
if (!sock->connect(dds->udp.ip.get_str(), dds->udp.port.get())) {
return false;
}
dds->udp.socket = sock;
Expand Down
11 changes: 10 additions & 1 deletion libraries/AP_DDS/AP_DDS_config.h
Original file line number Diff line number Diff line change
@@ -1,17 +1,26 @@
#pragma once

#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_Networking/AP_Networking_Config.h>

#ifndef AP_DDS_ENABLED
#define AP_DDS_ENABLED 1
#endif

// UDP only on SITL for now
#ifndef AP_DDS_UDP_ENABLED
#define AP_DDS_UDP_ENABLED AP_DDS_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#define AP_DDS_UDP_ENABLED AP_DDS_ENABLED && AP_NETWORKING_ENABLED
#endif

#include <AP_VisualOdom/AP_VisualOdom_config.h>
#ifndef AP_DDS_VISUALODOM_ENABLED
#define AP_DDS_VISUALODOM_ENABLED HAL_VISUALODOM_ENABLED && AP_DDS_ENABLED
#endif

#ifndef AP_DDS_DEFAULT_UDP_IP_ADDR
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#define AP_DDS_DEFAULT_UDP_IP_ADDR "192.168.13.2"
#else
#define AP_DDS_DEFAULT_UDP_IP_ADDR "127.0.0.1"
#endif
#endif
47 changes: 45 additions & 2 deletions libraries/AP_HAL/utility/Socket.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,8 @@
*/

#include <AP_HAL/AP_HAL.h>
#if HAL_OS_SOCKETS
#include <AP_Networking/AP_Networking_Config.h>
#if AP_NETWORKING_SOCKETS_ENABLED

#include "Socket.h"
#include <errno.h>
Expand All @@ -34,7 +35,9 @@ SocketAPM::SocketAPM(bool _datagram, int _fd) :
datagram(_datagram),
fd(_fd)
{
#ifdef FD_CLOEXEC
fcntl(fd, F_SETFD, FD_CLOEXEC);
#endif
if (!datagram) {
int one = 1;
setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
Expand All @@ -44,7 +47,11 @@ SocketAPM::SocketAPM(bool _datagram, int _fd) :
SocketAPM::~SocketAPM()
{
if (fd != -1) {
#if AP_NETWORKING_BACKEND_CHIBIOS
::lwip_close(fd);
#else
::close(fd);
#endif
fd = -1;
}
}
Expand All @@ -69,7 +76,11 @@ bool SocketAPM::connect(const char *address, uint16_t port)
struct sockaddr_in sockaddr;
make_sockaddr(address, port, sockaddr);

#if AP_NETWORKING_BACKEND_CHIBIOS
if (::lwip_connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
#else
if (::connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
#endif
return false;
}
return true;
Expand All @@ -85,7 +96,11 @@ bool SocketAPM::connect_timeout(const char *address, uint16_t port, uint32_t tim

set_blocking(false);

#if AP_NETWORKING_BACKEND_CHIBIOS
int ret = ::lwip_connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
#else
int ret = ::connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
#endif
if (ret == 0) {
// instant connect?
return true;
Expand Down Expand Up @@ -113,7 +128,11 @@ bool SocketAPM::bind(const char *address, uint16_t port)
struct sockaddr_in sockaddr;
make_sockaddr(address, port, sockaddr);

#if AP_NETWORKING_BACKEND_CHIBIOS
if (::lwip_bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
#else
if (::bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
#endif
return false;
}
return true;
Expand Down Expand Up @@ -148,15 +167,23 @@ bool SocketAPM::set_blocking(bool blocking) const
*/
bool SocketAPM::set_cloexec() const
{
#ifdef FD_CLOEXEC
return (fcntl(fd, F_SETFD, FD_CLOEXEC) != -1);
#else
return false;
#endif
}

/*
send some data
*/
ssize_t SocketAPM::send(const void *buf, size_t size) const
{
#if AP_NETWORKING_BACKEND_CHIBIOS
return ::lwip_send(fd, buf, size, 0);
#else
return ::send(fd, buf, size, 0);
#endif
}

/*
Expand All @@ -166,7 +193,11 @@ ssize_t SocketAPM::sendto(const void *buf, size_t size, const char *address, uin
{
struct sockaddr_in sockaddr;
make_sockaddr(address, port, sockaddr);
#if AP_NETWORKING_BACKEND_CHIBIOS
return ::lwip_sendto(fd, buf, size, 0, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
#else
return ::sendto(fd, buf, size, 0, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
#endif
}

/*
Expand All @@ -178,7 +209,11 @@ ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms)
return -1;
}
socklen_t len = sizeof(in_addr);
#if AP_NETWORKING_BACKEND_CHIBIOS
return ::lwip_recvfrom(fd, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr, &len);
#else
return ::recvfrom(fd, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr, &len);
#endif
}

/*
Expand Down Expand Up @@ -242,7 +277,11 @@ bool SocketAPM::pollout(uint32_t timeout_ms)
*/
bool SocketAPM::listen(uint16_t backlog) const
{
#if AP_NETWORKING_BACKEND_CHIBIOS
return ::lwip_listen(fd, (int)backlog) == 0;
#else
return ::listen(fd, (int)backlog) == 0;
#endif
}

/*
Expand All @@ -255,7 +294,11 @@ SocketAPM *SocketAPM::accept(uint32_t timeout_ms)
return nullptr;
}

#if AP_NETWORKING_BACKEND_CHIBIOS
int newfd = ::lwip_accept(fd, nullptr, nullptr);
#else
int newfd = ::accept(fd, nullptr, nullptr);
#endif
if (newfd == -1) {
return nullptr;
}
Expand All @@ -265,4 +308,4 @@ SocketAPM *SocketAPM::accept(uint32_t timeout_ms)
return new SocketAPM(false, newfd);
}

#endif // HAL_OS_SOCKETS
#endif // AP_NETWORKING_BACKEND_ANY
9 changes: 8 additions & 1 deletion libraries/AP_HAL/utility/Socket.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,9 @@
#pragma once

#include <AP_HAL/AP_HAL.h>
#include <AP_Networking/AP_Networking_Config.h>
#if AP_NETWORKING_SOCKETS_ENABLED

#if HAL_OS_SOCKETS

#include <fcntl.h>
Expand All @@ -28,6 +31,10 @@
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <sys/select.h>
#elif AP_NETWORKING_BACKEND_CHIBIOS
#include <AP_Networking/AP_Networking_ChibiOS.h>
#include <lwip/sockets.h>
#endif

class SocketAPM {
public:
Expand Down Expand Up @@ -72,4 +79,4 @@ class SocketAPM {
void make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr);
};

#endif // HAL_OS_SOCKETS
#endif // AP_NETWORKING_SOCKETS_ENABLED
24 changes: 24 additions & 0 deletions libraries/AP_Networking/AP_Networking.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,9 @@ extern const AP_HAL::HAL& hal;
#include <arpa/inet.h>
#endif

#if AP_NETWORKING_BACKEND_SITL
#include "AP_Networking_SITL.h"
#endif

const AP_Param::GroupInfo AP_Networking::var_info[] = {
// @Param: ENABLED
Expand Down Expand Up @@ -56,6 +59,20 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = {
// @Path: AP_Networking_macaddr.cpp
AP_SUBGROUPINFO(param.macaddr, "MACADDR", 6, AP_Networking, AP_Networking_MAC),

#if AP_NETWORKING_TESTS_ENABLED
// @Param: TESTS
// @DisplayName: Test enable flags
// @Description: Enable/Disable networking tests
// @Bitmask: 0:UDP echo test,1:TCP echo test
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("TESTS", 7, AP_Networking, param.tests, 0),

// @Group: TEST_IP
// @Path: AP_Networking_address.cpp
AP_SUBGROUPINFO(param.test_ipaddr, "TEST_IP", 8, AP_Networking, AP_Networking_IPV4),
#endif

AP_GROUPEND
};

Expand Down Expand Up @@ -100,6 +117,9 @@ void AP_Networking::init()
#if AP_NETWORKING_BACKEND_CHIBIOS
backend = new AP_Networking_ChibiOS(*this);
#endif
#if AP_NETWORKING_BACKEND_SITL
backend = new AP_Networking_SITL(*this);
#endif

if (backend == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: backend failed");
Expand All @@ -116,6 +136,10 @@ void AP_Networking::init()
announce_address_changes();

GCS_SEND_TEXT(MAV_SEVERITY_INFO,"NET: Initialized");

#if AP_NETWORKING_TESTS_ENABLED
start_tests();
#endif
}

/*
Expand Down
14 changes: 14 additions & 0 deletions libraries/AP_Networking/AP_Networking.h
Original file line number Diff line number Diff line change
Expand Up @@ -165,6 +165,10 @@ class AP_Networking
AP_Networking_MAC macaddr{AP_NETWORKING_DEFAULT_MAC_ADDR};
AP_Int8 enabled;
AP_Int32 options;
#if AP_NETWORKING_TESTS_ENABLED
AP_Int32 tests;
AP_Networking_IPV4 test_ipaddr{AP_NETWORKING_TEST_IP};
#endif
} param;

AP_Networking_Backend *backend;
Expand All @@ -173,6 +177,16 @@ class AP_Networking

private:
uint32_t announce_ms;

#if AP_NETWORKING_TESTS_ENABLED
enum {
TEST_UDP_CLIENT = (1U<<0),
TEST_TCP_CLIENT = (1U<<1),
};
void start_tests(void);
void test_UDP_client(void);
void test_TCP_client(void);
#endif // AP_NETWORKING_TESTS_ENABLED
};

namespace AP
Expand Down
29 changes: 0 additions & 29 deletions libraries/AP_Networking/AP_Networking_ChibiOS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -143,34 +143,5 @@ void AP_Networking_ChibiOS::update()
}
}

/*
send a UDP packet
*/
int32_t AP_Networking_ChibiOS::send_udp(struct udp_pcb *pcb, const ip4_addr_t &ip4_addr, const uint16_t port, const uint8_t* data, uint16_t data_len)
{
if (pcb == nullptr) {
return ERR_ARG;
}

data_len = (data == nullptr) ? 0 : data_len;

struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, data_len, PBUF_RAM);
if (p == nullptr) {
return ERR_MEM;
}

ip_addr_t dst;
ip_addr_copy_from_ip4(dst, ip4_addr);

if (data_len > 0) {
memcpy(p->payload, data, data_len);
}

const err_t err = udp_sendto(pcb, p, &dst, port);
pbuf_free(p);

return err == ERR_OK ? data_len : err;
}

#endif // AP_NETWORKING_BACKEND_CHIBIOS

1 change: 0 additions & 1 deletion libraries/AP_Networking/AP_Networking_ChibiOS.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@ class AP_Networking_ChibiOS : public AP_Networking_Backend

private:
bool allocate_buffers(void);
int32_t send_udp(struct udp_pcb *pcb, const struct ip4_addr &ip4_addr, const uint16_t port, const uint8_t* data, uint16_t data_len);

private:
struct lwipthread_opts *lwip_options;
Expand Down
Loading
Loading