From 52b16d062c2c9839c0aabadeb51582eb1e47af2a Mon Sep 17 00:00:00 2001 From: Zachary Lowell Date: Thu, 17 Nov 2022 15:51:01 -0600 Subject: [PATCH] uORB Remote Manager Update (#20623) --- Kconfig | 4 ++ boards/modalai/voxl2-slpi/default.px4board | 1 + boards/modalai/voxl2-slpi/src/board_config.h | 1 - boards/modalai/voxl2/default.px4board | 1 + boards/modalai/voxl2/src/board_config.h | 1 - platforms/common/Kconfig | 1 + platforms/common/uORB/Kconfig | 5 ++ platforms/common/uORB/uORBDeviceMaster.cpp | 4 -- platforms/common/uORB/uORBDeviceNode.cpp | 36 +++++-------- platforms/common/uORB/uORBDeviceNode.hpp | 6 +-- platforms/common/uORB/uORBManager.cpp | 55 +++++++++++++++++--- platforms/common/uORB/uORBManager.hpp | 22 ++++---- platforms/qurt/cmake/px4_impl_os.cmake | 2 - 13 files changed, 86 insertions(+), 53 deletions(-) create mode 100644 platforms/common/Kconfig create mode 100644 platforms/common/uORB/Kconfig diff --git a/Kconfig b/Kconfig index a6be4d1a8c..5d0239ec5b 100644 --- a/Kconfig +++ b/Kconfig @@ -190,3 +190,7 @@ endmenu menu "examples" source "src/examples/Kconfig" endmenu + +menu "platforms" +source "platforms/common/Kconfig" +endmenu diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index ccbefb55e1..b377bc4898 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -2,3 +2,4 @@ CONFIG_PLATFORM_QURT=y CONFIG_BOARD_TOOLCHAIN="qurt" CONFIG_MODULES_MUORB_SLPI=y CONFIG_SYSTEMCMDS_UORB=y +CONFIG_ORB_COMMUNICATOR=y diff --git a/boards/modalai/voxl2-slpi/src/board_config.h b/boards/modalai/voxl2-slpi/src/board_config.h index 6c57100198..d7a389ef9f 100644 --- a/boards/modalai/voxl2-slpi/src/board_config.h +++ b/boards/modalai/voxl2-slpi/src/board_config.h @@ -41,7 +41,6 @@ #define BOARD_HAS_NO_RESET #define BOARD_HAS_NO_BOOTLOADER -#define ORB_COMMUNICATOR 1 /* * I2C buses */ diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board index 5e26fb2fc7..47d77bce32 100644 --- a/boards/modalai/voxl2/default.px4board +++ b/boards/modalai/voxl2/default.px4board @@ -4,3 +4,4 @@ CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu" CONFIG_MODULES_MUORB_APPS=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_UORB=y +CONFIG_ORB_COMMUNICATOR=y diff --git a/boards/modalai/voxl2/src/board_config.h b/boards/modalai/voxl2/src/board_config.h index eb67c2e8f6..4f87f443eb 100644 --- a/boards/modalai/voxl2/src/board_config.h +++ b/boards/modalai/voxl2/src/board_config.h @@ -41,7 +41,6 @@ #define BOARD_HAS_NO_RESET #define BOARD_HAS_NO_BOOTLOADER -#define ORB_COMMUNICATOR 1 // Define this as empty since there are no I2C buses #define BOARD_I2C_BUS_CLOCK_INIT diff --git a/platforms/common/Kconfig b/platforms/common/Kconfig new file mode 100644 index 0000000000..6353836988 --- /dev/null +++ b/platforms/common/Kconfig @@ -0,0 +1 @@ +rsource "*/Kconfig" diff --git a/platforms/common/uORB/Kconfig b/platforms/common/uORB/Kconfig new file mode 100644 index 0000000000..67f492b1f8 --- /dev/null +++ b/platforms/common/uORB/Kconfig @@ -0,0 +1,5 @@ +menuconfig ORB_COMMUNICATOR + bool "orb communicator" + default n + ---help--- + Enable support for the uorb communicator for distributed platforms diff --git a/platforms/common/uORB/uORBDeviceMaster.cpp b/platforms/common/uORB/uORBDeviceMaster.cpp index e753a270da..c39e4572db 100644 --- a/platforms/common/uORB/uORBDeviceMaster.cpp +++ b/platforms/common/uORB/uORBDeviceMaster.cpp @@ -36,10 +36,6 @@ #include "uORBManager.hpp" #include "uORBUtils.hpp" -#ifdef ORB_COMMUNICATOR -#include "uORBCommunicator.hpp" -#endif /* ORB_COMMUNICATOR */ - #include #include diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index 25dc68a04a..15bbcd5d56 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -38,9 +38,9 @@ #include "SubscriptionCallback.hpp" -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR #include "uORBCommunicator.hpp" -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ #if defined(__PX4_NUTTX) #include @@ -304,7 +304,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v return PX4_ERROR; } -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR /* * if the write is successful, send the data over the Multi-ORB link */ @@ -317,7 +317,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v } } -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ return PX4_OK; } @@ -346,7 +346,7 @@ int uORB::DeviceNode::unadvertise(orb_advert_t handle) return PX4_OK; } -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta) { uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); @@ -357,19 +357,7 @@ int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta) return -1; } - -/* -//TODO: Check if we need this since we only unadvertise when things all shutdown and it doesn't actually remove the device -int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta) -{ - uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); - if (ch != nullptr && meta != nullptr) { - return ch->topic_unadvertised(meta->o_name); - } - return -1; -} -*/ -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ px4_pollevent_t uORB::DeviceNode::poll_state(cdev::file_t *filp) @@ -413,7 +401,7 @@ void uORB::DeviceNode::add_internal_subscriber() lock(); _subscriber_count++; -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); if (ch != nullptr && _subscriber_count > 0) { @@ -421,7 +409,7 @@ void uORB::DeviceNode::add_internal_subscriber() ch->add_subscription(_meta->o_name, 1); } else -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ { unlock(); @@ -433,7 +421,7 @@ void uORB::DeviceNode::remove_internal_subscriber() lock(); _subscriber_count--; -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); if (ch != nullptr && _subscriber_count == 0) { @@ -441,13 +429,13 @@ void uORB::DeviceNode::remove_internal_subscriber() ch->remove_subscription(_meta->o_name); } else -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ { unlock(); } } -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz) { // if there is already data in the node, send this out to @@ -490,7 +478,7 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data return PX4_OK; } -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ int uORB::DeviceNode::update_queue_size(unsigned int queue_size) { diff --git a/platforms/common/uORB/uORBDeviceNode.hpp b/platforms/common/uORB/uORBDeviceNode.hpp index bff59b3811..5ffb18b58d 100644 --- a/platforms/common/uORB/uORBDeviceNode.hpp +++ b/platforms/common/uORB/uORBDeviceNode.hpp @@ -41,6 +41,7 @@ #include #include #include +#include namespace uORB { @@ -122,9 +123,8 @@ public: static int unadvertise(orb_advert_t handle); -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR static int16_t topic_advertised(const orb_metadata *meta); - //static int16_t topic_unadvertised(const orb_metadata *meta); /** * processes a request for add subscription from remote @@ -145,7 +145,7 @@ public: * processed the received data message from remote. */ int16_t process_received_message(int32_t length, uint8_t *data); -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ /** * Add the subscriber to the node's list of subscriber. If there is diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 1dce82b319..030a38df1d 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -48,6 +48,10 @@ #include "uORBUtils.hpp" #include "uORBManager.hpp" +#ifdef CONFIG_ORB_COMMUNICATOR +pthread_mutex_t uORB::Manager::_communicator_mutex = PTHREAD_MUTEX_INITIALIZER; +#endif + uORB::Manager *uORB::Manager::_Instance = nullptr; bool uORB::Manager::initialize() @@ -258,7 +262,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) } } -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR /* * Generate the path to the node and try to open it. @@ -300,7 +304,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) } } -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ return ret; } @@ -360,10 +364,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, return nullptr; } -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR // For remote systems call over and inform them uORB::DeviceNode::topic_advertised(meta); -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ /* the advertiser may perform an initial publish to initialise the object */ if (data != nullptr) { @@ -611,7 +615,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i return fd; } -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel) { _comm_channel = channel; @@ -623,18 +627,53 @@ void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel) uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator() { - return _comm_channel; + pthread_mutex_lock(&_communicator_mutex); + uORBCommunicator::IChannel *temp = _comm_channel; + pthread_mutex_unlock(&_communicator_mutex); + + return temp; } int16_t uORB::Manager::process_remote_topic(const char *topic_name, bool isAdvertisement) { + PX4_DEBUG("entering process_remote_topic: name: %s", topic_name); + int16_t rc = 0; - if (isAdvertisement) { + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, topic_name); + DeviceMaster *device_master = get_device_master(); + + if (ret == OK && device_master && isAdvertisement) { + uORB::DeviceNode *node = device_master->getDeviceNode(nodepath); + + if (node) { + node->mark_as_advertised(); + _remote_topics.insert(topic_name); + return rc; + } + } + + // Didn't find a node so we need to create it via an advertisement + const struct orb_metadata *const *topic_list = orb_get_topics(); + orb_id_t topic_ptr = nullptr; + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(topic_list[i]->o_name, topic_name) == 0) { + topic_ptr = topic_list[i]; + break; + } + } + + if (topic_ptr) { + PX4_INFO("Advertising remote topic %s", topic_name); _remote_topics.insert(topic_name); + orb_advertise(topic_ptr, nullptr); } else { + PX4_INFO("process_remote_topic meta not found for %s\n", topic_name); _remote_topics.erase(topic_name); + rc = -1; } return rc; @@ -723,7 +762,7 @@ bool uORB::Manager::is_remote_subscriber_present(const char *messageName) return _remote_subscriber_topics.find(messageName); } -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ #ifdef ORB_USE_PUBLISHER_RULES diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 1fbc779da1..b466f0cd22 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -40,11 +40,12 @@ #include // For ORB_ID enum #include +#include -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR #include "ORBSet.hpp" #include "uORBCommunicator.hpp" -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ namespace uORB { @@ -159,9 +160,9 @@ typedef enum { * uORB Api's. */ class uORB::Manager -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR : public uORBCommunicator::IChannelRxHandler -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ { public: // public interfaces for this class. @@ -464,7 +465,7 @@ public: static bool is_advertised(const void *node_handle); #endif -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR /** * Method to set the uORBCommunicator::IChannel instance. * @param comm_channel @@ -485,7 +486,7 @@ public: * for a given topic */ bool is_remote_subscriber_present(const char *messageName); -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ private: // class methods @@ -500,13 +501,14 @@ private: // class methods private: // data members static Manager *_Instance; -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR // the communicator channel instance. uORBCommunicator::IChannel *_comm_channel{nullptr}; + static pthread_mutex_t _communicator_mutex; ORBSet _remote_subscriber_topics; ORBSet _remote_topics; -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ DeviceMaster *_device_master{nullptr}; @@ -514,7 +516,7 @@ private: //class methods Manager(); virtual ~Manager(); -#ifdef ORB_COMMUNICATOR +#ifdef CONFIG_ORB_COMMUNICATOR /** * Interface to process a received topic from remote. * @param topic_name @@ -570,7 +572,7 @@ private: //class methods * otherwise = failure. */ virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data); -#endif /* ORB_COMMUNICATOR */ +#endif /* CONFIG_ORB_COMMUNICATOR */ #ifdef ORB_USE_PUBLISHER_RULES diff --git a/platforms/qurt/cmake/px4_impl_os.cmake b/platforms/qurt/cmake/px4_impl_os.cmake index 62ce1738d6..fc5a67a6e6 100644 --- a/platforms/qurt/cmake/px4_impl_os.cmake +++ b/platforms/qurt/cmake/px4_impl_os.cmake @@ -50,8 +50,6 @@ include(qurt_reqs) include_directories(${HEXAGON_SDK_INCLUDES}) -add_definitions(-DORB_COMMUNICATOR) - set(DISABLE_PARAMS_MODULE_SCOPING TRUE) #=============================================================================