dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

uORB Remote Manager Update (#20623)

This commit is contained in:
Zachary Lowell 2022-11-17 15:51:01 -06:00 committed by GitHub
parent 8b7c074680
commit 52b16d062c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
13 changed files with 86 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Kconfig
View File

@ -190,3 +190,7 @@ endmenu
menu "examples"
source "src/examples/Kconfig"
endmenu
menu "platforms"
source "platforms/common/Kconfig"
endmenu

1
boards/modalai/voxl2-slpi/default.px4board
View File

@ -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

1
boards/modalai/voxl2-slpi/src/board_config.h
View File

@ -41,7 +41,6 @@
#define BOARD_HAS_NO_RESET
#define BOARD_HAS_NO_BOOTLOADER
#define ORB_COMMUNICATOR 1
/*
* I2C buses
*/

1
boards/modalai/voxl2/default.px4board
View File

@ -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

1
boards/modalai/voxl2/src/board_config.h
View File

@ -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

1
platforms/common/Kconfig Normal file
View File

@ -0,0 +1 @@
rsource "*/Kconfig"

5
platforms/common/uORB/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig ORB_COMMUNICATOR
bool "orb communicator"
default n
---help---
Enable support for the uorb communicator for distributed platforms

4
platforms/common/uORB/uORBDeviceMaster.cpp
View File

@ -36,10 +36,6 @@
#include "uORBManager.hpp"
#include "uORBUtils.hpp"
#ifdef ORB_COMMUNICATOR
#include "uORBCommunicator.hpp"
#endif /* ORB_COMMUNICATOR */
#include <px4_platform_common/sem.hpp>
#include <systemlib/px4_macros.h>

36
platforms/common/uORB/uORBDeviceNode.cpp
View File

@ -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 <nuttx/mm/mm.h>
@ -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)
{

6
platforms/common/uORB/uORBDeviceNode.hpp
View File

@ -41,6 +41,7 @@
#include <containers/IntrusiveSortedList.hpp>
#include <containers/List.hpp>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/px4_config.h>
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

55
platforms/common/uORB/uORBManager.cpp
View File

@ -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

22
platforms/common/uORB/uORBManager.hpp
View File

@ -40,11 +40,12 @@
#include <uORB/topics/uORBTopics.hpp> // For ORB_ID enum
#include <stdint.h>
#include <px4_platform_common/px4_config.h>
#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

2
platforms/qurt/cmake/px4_impl_os.cmake
View File

@ -50,8 +50,6 @@ include(qurt_reqs)
include_directories(${HEXAGON_SDK_INCLUDES})
add_definitions(-DORB_COMMUNICATOR)
set(DISABLE_PARAMS_MODULE_SCOPING TRUE)
#=============================================================================