yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Enabled UDP in NuttX microRTPS build

Browse Source 
Added commandline argument to change microRTPS ip address
This commit is contained in:
Peter van der Perk
2019-10-29 09:42:58 +01:00
committed by Nuno Marques
parent 375fc4a75c
commit 07eb3d301b
5 changed files with 30 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
msg/templates/urtps/microRTPS_agent.cpp.em
View File

@@ -80,6 +80,7 @@ recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumer
#define WAIT_CNST 2 #define WAIT_CNST 2
#define DEFAULT_RECV_PORT 2020 #define DEFAULT_RECV_PORT 2020
#define DEFAULT_SEND_PORT 2019 #define DEFAULT_SEND_PORT 2019
#define DEFAULT_IP "127.0.0.1"
using namespace eprosima; using namespace eprosima;
using namespace eprosima::fastrtps; using namespace eprosima::fastrtps;
@@ -119,6 +120,7 @@ struct options {
int poll_ms = POLL_MS; int poll_ms = POLL_MS;
uint16_t recv_port = DEFAULT_RECV_PORT; uint16_t recv_port = DEFAULT_RECV_PORT;
uint16_t send_port = DEFAULT_SEND_PORT; uint16_t send_port = DEFAULT_SEND_PORT;
char ip[16] = DEFAULT_IP;
} _options; } _options;
static void usage(const char *name) static void usage(const char *name)
@@ -130,7 +132,8 @@ static void usage(const char *name)
" -b <baudrate> UART device baudrate. Default 460800\n" " -b <baudrate> UART device baudrate. Default 460800\n"
" -p <poll_ms> Time in ms to poll over UART. Default 1ms\n" " -p <poll_ms> Time in ms to poll over UART. Default 1ms\n"
" -r <reception port> UDP port for receiving. Default 2019\n" " -r <reception port> UDP port for receiving. Default 2019\n"
" -s <sending port> UDP port for sending. Default 2020\n", " -s <sending port> UDP port for sending. Default 2020\n"
" -i <ip_address> Target IP for UDP. Default 127.0.0.1\n",
name); name);
} }
@@ -147,7 +150,7 @@ static int parse_options(int argc, char **argv)
{ {
int ch; int ch;
while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:")) != EOF) while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:i:")) != EOF)
{ {
switch (ch) switch (ch)
{ {
@@ -160,6 +163,7 @@ static int parse_options(int argc, char **argv)
case 'p': _options.poll_ms = strtol(optarg, nullptr, 10); break; case 'p': _options.poll_ms = strtol(optarg, nullptr, 10); break;
case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break; case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break;
case 's': _options.send_port = strtoul(optarg, nullptr, 10); break; case 's': _options.send_port = strtoul(optarg, nullptr, 10); break;
case 'i': if (nullptr != optarg) strcpy(_options.ip, optarg); break;
default: default:
usage(argv[0]); usage(argv[0]);
return -1; return -1;
@@ -237,9 +241,9 @@ int main(int argc, char** argv)
break; break;
case options::eTransports::UDP: case options::eTransports::UDP:
{ {
transport_node = new UDP_node(_options.recv_port, _options.send_port); transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port);
printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dus\n\n", printf("\nUDP transport: ip address: %s; recv port: %u; send port: %u; sleep: %dus\n\n",
_options.recv_port, _options.send_port, _options.sleep_us); _options.ip, _options.recv_port, _options.send_port, _options.sleep_us);
} }
break; break;
default: default:

17
msg/templates/urtps/microRTPS_transport.cpp
View File

@@ -535,12 +535,15 @@ bool UART_node::baudrate_to_speed(uint32_t bauds, speed_t *speed)
return true; return true;
} }
UDP_node::UDP_node(uint16_t _udp_port_recv, uint16_t _udp_port_send): UDP_node::UDP_node(const char* _udp_ip, uint16_t _udp_port_recv, uint16_t _udp_port_send):
sender_fd(-1), sender_fd(-1),
receiver_fd(-1), receiver_fd(-1),
udp_port_recv(_udp_port_recv), udp_port_recv(_udp_port_recv),
udp_port_send(_udp_port_send) udp_port_send(_udp_port_send)
{ {
if (nullptr != _udp_ip) {
strcpy(udp_ip, _udp_ip);
}
} }
UDP_node::~UDP_node() UDP_node::~UDP_node()
@@ -564,7 +567,7 @@ bool UDP_node::fds_OK()
int UDP_node::init_receiver(uint16_t udp_port) int UDP_node::init_receiver(uint16_t udp_port)
{ {
#ifndef __PX4_NUTTX #if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX))
// udp socket data // udp socket data
memset((char *)&receiver_inaddr, 0, sizeof(receiver_inaddr)); memset((char *)&receiver_inaddr, 0, sizeof(receiver_inaddr));
receiver_inaddr.sin_family = AF_INET; receiver_inaddr.sin_family = AF_INET;
@@ -604,7 +607,7 @@ int UDP_node::init_receiver(uint16_t udp_port)
int UDP_node::init_sender(uint16_t udp_port) int UDP_node::init_sender(uint16_t udp_port)
{ {
#ifndef __PX4_NUTTX #if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX))
if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
#ifndef PX4_ERR #ifndef PX4_ERR
@@ -619,7 +622,7 @@ int UDP_node::init_sender(uint16_t udp_port)
sender_outaddr.sin_family = AF_INET; sender_outaddr.sin_family = AF_INET;
sender_outaddr.sin_port = htons(udp_port); sender_outaddr.sin_port = htons(udp_port);
if (inet_aton("127.0.0.1", &sender_outaddr.sin_addr) == 0) { if (inet_aton(udp_ip, &sender_outaddr.sin_addr) == 0) {
#ifndef PX4_ERR #ifndef PX4_ERR
printf("inet_aton() failed"); printf("inet_aton() failed");
#else #else
@@ -635,7 +638,7 @@ int UDP_node::init_sender(uint16_t udp_port)
uint8_t UDP_node::close() uint8_t UDP_node::close()
{ {
#ifndef __PX4_NUTTX #if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX))
if (sender_fd != -1) { if (sender_fd != -1) {
#ifndef PX4_WARN #ifndef PX4_WARN
@@ -670,7 +673,7 @@ ssize_t UDP_node::node_read(void *buffer, size_t len)
} }
int ret = 0; int ret = 0;
#ifndef __PX4_NUTTX #if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX))
// Blocking call // Blocking call
static socklen_t addrlen = sizeof(receiver_outaddr); static socklen_t addrlen = sizeof(receiver_outaddr);
ret = recvfrom(receiver_fd, buffer, len, 0, (struct sockaddr *) &receiver_outaddr, &addrlen); ret = recvfrom(receiver_fd, buffer, len, 0, (struct sockaddr *) &receiver_outaddr, &addrlen);
@@ -685,7 +688,7 @@ ssize_t UDP_node::node_write(void *buffer, size_t len)
} }
int ret = 0; int ret = 0;
#ifndef __PX4_NUTTX #if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX))
ret = sendto(sender_fd, buffer, len, 0, (struct sockaddr *)&sender_outaddr, sizeof(sender_outaddr)); ret = sendto(sender_fd, buffer, len, 0, (struct sockaddr *)&sender_outaddr, sizeof(sender_outaddr));
#endif /* __PX4_NUTTX */ #endif /* __PX4_NUTTX */
return ret; return ret;

3
msg/templates/urtps/microRTPS_transport.h
View File

@@ -113,7 +113,7 @@ protected:
class UDP_node: public Transport_node class UDP_node: public Transport_node
{ {
public: public:
UDP_node(uint16_t udp_port_recv, uint16_t udp_port_send); UDP_node(const char* _udp_ip, uint16_t udp_port_recv, uint16_t udp_port_send);
virtual ~UDP_node(); virtual ~UDP_node();
int init(); int init();
@@ -128,6 +128,7 @@ protected:
int sender_fd; int sender_fd;
int receiver_fd; int receiver_fd;
char udp_ip[16] = {};
uint16_t udp_port_recv; uint16_t udp_port_recv;
uint16_t udp_port_send; uint16_t udp_port_send;
struct sockaddr_in sender_outaddr; struct sockaddr_in sender_outaddr;

2
src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h
View File

@@ -62,6 +62,7 @@
#endif #endif
#define DEVICE "/dev/ttyACM0" #define DEVICE "/dev/ttyACM0"
#define POLL_MS 1 #define POLL_MS 1
#define IP "127.0.0.1"
#define DEFAULT_RECV_PORT 2019 #define DEFAULT_RECV_PORT 2019
#define DEFAULT_SEND_PORT 2020 #define DEFAULT_SEND_PORT 2020
@@ -85,6 +86,7 @@ struct options {
int sleep_ms = SLEEP_MS; int sleep_ms = SLEEP_MS;
struct baudtype baudrate = {.code = BAUDRATE, .val = BAUDRATE_VAL}; struct baudtype baudrate = {.code = BAUDRATE, .val = BAUDRATE_VAL};
int poll_ms = POLL_MS; int poll_ms = POLL_MS;
char ip[16] = IP;
uint16_t recv_port = DEFAULT_RECV_PORT; uint16_t recv_port = DEFAULT_RECV_PORT;
uint16_t send_port = DEFAULT_SEND_PORT; uint16_t send_port = DEFAULT_SEND_PORT;
}; };

11
src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
View File

@@ -84,6 +84,7 @@ static void usage(const char *name)
PRINT_MODULE_USAGE_PARAM_INT('w', 1, 1, 1000, "Time in ms for which each iteration sleeps", true); PRINT_MODULE_USAGE_PARAM_INT('w', 1, 1, 1000, "Time in ms for which each iteration sleeps", true);
PRINT_MODULE_USAGE_PARAM_INT('r', 2019, 0, 65536, "Select UDP Network Port for receiving (local)", true); PRINT_MODULE_USAGE_PARAM_INT('r', 2019, 0, 65536, "Select UDP Network Port for receiving (local)", true);
PRINT_MODULE_USAGE_PARAM_INT('s', 2020, 0, 65536, "Select UDP Network Port for sending (remote)", true); PRINT_MODULE_USAGE_PARAM_INT('s', 2020, 0, 65536, "Select UDP Network Port for sending (remote)", true);
PRINT_MODULE_USAGE_PARAM_STRING('i', "127.0.0.1", "<x.x.x.x>", "Select IP address (remote)", true);
PRINT_MODULE_USAGE_COMMAND("stop"); PRINT_MODULE_USAGE_COMMAND("stop");
PRINT_MODULE_USAGE_COMMAND("status"); PRINT_MODULE_USAGE_COMMAND("status");
@@ -108,7 +109,7 @@ static int parse_options(int argc, char *argv[])
int myoptind = 1; int myoptind = 1;
const char *myoptarg = nullptr; const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "t:d:u:l:w:b:p:r:s:", &myoptind, &myoptarg)) != EOF) { while ((ch = px4_getopt(argc, argv, "t:d:u:l:w:b:p:r:s:i:", &myoptind, &myoptarg)) != EOF) {
switch (ch) { switch (ch) {
case 't': _options.transport = strcmp(myoptarg, "UDP") == 0 ? case 't': _options.transport = strcmp(myoptarg, "UDP") == 0 ?
options::eTransports::UDP options::eTransports::UDP
@@ -130,6 +131,8 @@ static int parse_options(int argc, char *argv[])
case 's': _options.send_port = strtoul(myoptarg, nullptr, 10); break; case 's': _options.send_port = strtoul(myoptarg, nullptr, 10); break;
case 'i': if (nullptr != myoptarg) strcpy(_options.ip, myoptarg); break;
default: default:
usage(argv[1]); usage(argv[1]);
return -1; return -1;
@@ -166,9 +169,9 @@ static int micrortps_start(int argc, char *argv[])
break; break;
case options::eTransports::UDP: { case options::eTransports::UDP: {
transport_node = new UDP_node(_options.recv_port, _options.send_port); transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port);
PX4_INFO("UDP transport: recv port: %u; send port: %u; sleep: %dms", PX4_INFO("UDP transport: ip address: %s; recv port: %u; send port: %u; sleep: %dms",
_options.recv_port, _options.send_port, _options.sleep_ms); _options.ip, _options.recv_port, _options.send_port, _options.sleep_ms);
} }
break; break;