From c7f67a3328c31454c6da76fd5b7d12668c6215ed Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Tue, 7 Mar 2023 16:42:23 -0800 Subject: [PATCH] microdds_client: remove multi-instances supports, add agent IP as PX4 parameter - multi-instances support is removed from the parameter definitions. - XRCE_DDS_AG_IP allows to define the agent IP when udp transport is used. The parameter is used by default if the module is started without the -h argument. - XRCE_DDS_PRT allows to define the agent listning udp port when the udp transport is uded. The parameter is used by default if the module is started without the -p argument. - Tools/convert_ip.py assists in converting ips in decimal dot notation into int32 notation. Signed-off-by: Beniamino Pozzan --- ROMFS/px4fmu_common/init.d-posix/rcS | 2 +- Tools/convert_ip.py | 53 ++++++++++++++++ .../microdds_client/microdds_client.cpp | 61 +++++++++++++++---- src/modules/microdds_client/microdds_client.h | 11 ++++ src/modules/microdds_client/module.yaml | 40 ++++++------ 5 files changed, 134 insertions(+), 33 deletions(-) create mode 100644 Tools/convert_ip.py diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 3b7210d89f..179f57a817 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -277,7 +277,7 @@ then # Override namespace if environment variable is defined microdds_ns="-n $PX4_MICRODDS_NS" fi -microdds_client start -t udp -p 8888 $microdds_ns +microdds_client start -t udp -h 127.0.0.1 -p 8888 $microdds_ns if param greater -s MNT_MODE_IN -1 then diff --git a/Tools/convert_ip.py b/Tools/convert_ip.py new file mode 100644 index 0000000000..86579d2eb5 --- /dev/null +++ b/Tools/convert_ip.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 + +""" +Converts IP address given in decimal dot notation to int32 to be used in XRCE_DDS_0_CFG parameter +and vice-versa + +@author: Beniamino Pozzan (beniamino.pozzan@phd.unipd.it) +""" + + +import argparse + +parser = argparse.ArgumentParser( + prog = 'convert_ip', + description = 'converts IP to int32 and viceversa' + ) +parser.add_argument('input', + type=str, + help='IP address to convert') +parser.add_argument('-r','--reverse', + action='store_true', + help='converts from int32 to dot decimal') + +args = parser.parse_args() + +if( args.reverse == False ): + + try: + ip_parts = [int(x) for x in args.input.split('.')] + except: + raise ValueError("Not a valid IP") + if( len(ip_parts)!=4 ): + raise ValueError("Not a valid IP") + if( not all(x>=0 and x<255 for x in ip_parts) ): + raise ValueError("Not a valid IP") + + ip = (ip_parts[0]<<24) + (ip_parts[1]<<16) + (ip_parts[2]<<8) + ip_parts[3] + + if(ip & 0x80000000): + ip = -0x100000000 + ip + + print("the int32 conversion of", args.input, "is") + print(ip) + +else: + try: + ip = int(args.input) + except: + raise ValueError("Not a valid IP") + if(ip < 0): + ip = ip + 0x100000000 + print("the decimal dot conversion of", args.input, "is") + print('{}.{}.{}.{}'.format(ip>>24, (ip>>16)&0xff, (ip>>8)&0xff, ip&0xff)) diff --git a/src/modules/microdds_client/microdds_client.cpp b/src/modules/microdds_client/microdds_client.cpp index 42bcb874c5..4c5721908a 100644 --- a/src/modules/microdds_client/microdds_client.cpp +++ b/src/modules/microdds_client/microdds_client.cpp @@ -79,7 +79,7 @@ void on_time(uxrSession *session, int64_t current_time, int64_t received_timesta } } -MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *host, +MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *agent_ip, const char *port, bool localhost_only, bool custom_participant, const char *client_namespace) : ModuleParams(nullptr), _localhost_only(localhost_only), _custom_participant(custom_participant), @@ -122,9 +122,11 @@ MicroddsClient::MicroddsClient(Transport transport, const char *device, int baud #if defined(MICRODDS_CLIENT_UDP) _transport_udp = new uxrUDPTransport(); + strncpy(_port, port, PORT_MAX_LENGTH - 1); + strncpy(_agent_ip, agent_ip, AGENT_IP_MAX_LENGTH - 1); if (_transport_udp) { - if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, host, port)) { + if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, _agent_ip, _port)) { _comm = &_transport_udp->comm; _fd = _transport_udp->platform.poll_fd.fd; @@ -546,8 +548,26 @@ int MicroddsClient::task_spawn(int argc, char *argv[]) int MicroddsClient::print_status() { PX4_INFO("Running, %s", _connected ? "connected" : "disconnected"); - PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate); - PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate); +#if defined(MICRODDS_CLIENT_UDP) + + if (_transport_udp != nullptr) { + PX4_INFO("Using transport: udp"); + PX4_INFO("Agent IP: %s", _agent_ip); + PX4_INFO("Agent port: %s", _port); + + } + +#endif + + if (_transport_serial != nullptr) { + PX4_INFO("Using transport: serial"); + } + + if (_connected) { + PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate); + PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate); + } + return 0; } @@ -558,19 +578,37 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; + char port[PORT_MAX_LENGTH]; + char agent_ip[AGENT_IP_MAX_LENGTH]; + #if defined(MICRODDS_CLIENT_UDP) Transport transport = Transport::Udp; + int32_t port_i = 0; + param_get(param_find("XRCE_DDS_PRT"), &port_i); + + if (port_i < 0 || port_i > 65535) { + PX4_ERR("port must be between 0 and 65535"); + return nullptr; + } + + snprintf(port, PORT_MAX_LENGTH, "%u", (uint16_t)port_i); + + int32_t ip_i = 0; + param_get(param_find("XRCE_DDS_AG_IP"), &ip_i); + snprintf(agent_ip, AGENT_IP_MAX_LENGTH, "%u.%u.%u.%u", static_cast(((ip_i) >> 24) & 0xff), + static_cast(((ip_i) >> 16) & 0xff), + static_cast(((ip_i) >> 8) & 0xff), + static_cast(ip_i & 0xff)); + #else Transport transport = Transport::Serial; #endif const char *device = nullptr; int baudrate = 921600; - const char *port = "8888"; bool localhost_only = false; bool custom_participant = false; - const char *ip = "127.0.0.1"; const char *client_namespace = nullptr;//"px4"; @@ -605,11 +643,11 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) #if defined(MICRODDS_CLIENT_UDP) case 'h': - ip = myoptarg; + snprintf(agent_ip, AGENT_IP_MAX_LENGTH, "%s", myoptarg); break; case 'p': - port = myoptarg; + snprintf(port, PORT_MAX_LENGTH, "%s", myoptarg); break; case 'l': @@ -647,7 +685,8 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) } } - return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only, custom_participant, client_namespace); + return new MicroddsClient(transport, device, baudrate, agent_ip, port, localhost_only, custom_participant, + client_namespace); } int MicroddsClient::print_usage(const char *reason) @@ -671,8 +710,8 @@ $ microdds_client start -t udp -h 127.0.0.1 -p 15555 PRINT_MODULE_USAGE_PARAM_STRING('t', "udp", "serial|udp", "Transport protocol", true); PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "serial device", true); PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:)", true); - PRINT_MODULE_USAGE_PARAM_STRING('h', "127.0.0.1", "", "Host IP", true); - PRINT_MODULE_USAGE_PARAM_INT('p', 8888, 0, 65535, "Remote Port", true); + PRINT_MODULE_USAGE_PARAM_STRING('h', "nullptr", "", "Agent IP", true); + PRINT_MODULE_USAGE_PARAM_INT('p', -1, 0, 65535, "Agent listening port", true); PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true); PRINT_MODULE_USAGE_PARAM_FLAG('c', "Use custom participant config (profile_name=\"px4_participant\")", true); PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Client DDS namespace", true); diff --git a/src/modules/microdds_client/microdds_client.h b/src/modules/microdds_client/microdds_client.h index 8e5708d36d..1cd9700a78 100644 --- a/src/modules/microdds_client/microdds_client.h +++ b/src/modules/microdds_client/microdds_client.h @@ -78,6 +78,17 @@ private: const bool _custom_participant; const char *_client_namespace; + + // max port characters (5+'\0') + static const uint8_t PORT_MAX_LENGTH = 6; + // max agent ip characters (15+'\0') + static const uint8_t AGENT_IP_MAX_LENGTH = 16; + +#if defined(CONFIG_NET) || defined(__PX4_POSIX) + char _port[PORT_MAX_LENGTH]; + char _agent_ip[AGENT_IP_MAX_LENGTH]; +#endif + SendTopicsSubs *_subs{nullptr}; RcvTopicsPubs *_pubs{nullptr}; diff --git a/src/modules/microdds_client/module.yaml b/src/modules/microdds_client/module.yaml index 0b30eb2319..9dbca1b9ee 100644 --- a/src/modules/microdds_client/module.yaml +++ b/src/modules/microdds_client/module.yaml @@ -1,16 +1,3 @@ - - -# parameters to auto start -# mode (normal, minimal) -# UDP port -# max rate -# DDS DOMAIN ID -# - - -# multiple instances? - - module_name: Micro XRCE-DDS serial_config: - command: | @@ -22,13 +9,8 @@ serial_config: microdds_client start ${XRCE_DDS_ARGS} port_config_param: - name: XRCE_DDS_${i}_CFG + name: XRCE_DDS_CFG group: Micro XRCE-DDS - # MAVLink instances: - # 0: Telem1 Port (Telemetry Link) - # 1: Telem2 Port (Companion Link). Disabled by default to reduce RAM usage - # 2: Board-specific / no fixed function or port - #default: [TEL1, "", ""] supports_networking: true parameters: @@ -46,7 +28,7 @@ parameters: XRCE_DDS_KEY: description: - short: XRCE DDS key + short: XRCE DDS Session key long: | XRCE DDS key, must be different from zero. In a single agent - multi client configuration, each client @@ -56,13 +38,29 @@ parameters: reboot_required: true default: 1 - XRCE_DDS_UDP_PRT: + XRCE_DDS_PRT: description: short: Micro DDS UDP Port long: | If ethernet enabled and selected as configuration for micro DDS, selected udp port will be set and used. type: int32 + min: 0 + max: 65535 reboot_required: true default: 8888 requires_ethernet: true + + XRCE_DDS_AG_IP: + description: + short: Micro DDS Agent IP address + long: | + If ethernet enabled and selected as configuration for micro DDS, + selected Agent IP address will be set and used. + Decimal dot notation is not supported. IP address must be provided + in int32 format. For example, 192.168.1.2 is mapped to -1062731518; + 127.0.0.1 is mapped to 2130706433. + type: int32 + reboot_required: true + default: 2130706433 + requires_ethernet: true