fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 04:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add IOCTL interface to uORBManager for nuttx protected/kernel build split

Browse Source 
When building uORB for NuttX flat build, or for some other target, everything
works as before.

When building uORB for NuttX protected or kernel build, this does the following:
- The kernel side uORB library reigsters a boardctl handler for calls from userspace
  and services the boardctl_ioctls by calling the actual uORB functions
- For user mode binaries, the uORBManager acts as a proxy, making boardctl_ioctl calls to the
  kernel side

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen
2022-02-14 09:10:49 +01:00
committed by Beat Küng
parent 8b9a856cf7
commit 36d440f895
5 changed files with 657 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/common/uORB/uORBManager.cpp
+138
View File
@@ -40,6 +40,10 @@
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__)
#include <px4_platform/board_ctrl.h>
#endif
#include "uORBDeviceNode.hpp"
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
@@ -52,6 +56,9 @@ bool uORB::Manager::initialize()
_Instance = new uORB::Manager();
}
#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__)
px4_register_boardct_ioctl(_ORBIOCBASE, orb_ioctl);
#endif
return _Instance != nullptr;
}
@@ -103,6 +110,124 @@ uORB::DeviceMaster *uORB::Manager::get_device_master()
return _device_master;
}
#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__)
int uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg)
{
int ret = PX4_OK;
switch (cmd) {
case ORBIOCDEVEXISTS: {
orbiocdevexists_t *data = (orbiocdevexists_t *)arg;
if (data->check_advertised) {
data->ret = uORB::Manager::orb_exists(get_orb_meta(data->orb_id), data->instance);
} else {
data->ret = uORB::Manager::orb_device_node_exists(data->orb_id, data->instance) ? PX4_OK : PX4_ERROR;
}
}
break;
case ORBIOCDEVADVERTISE: {
orbiocdevadvertise_t *data = (orbiocdevadvertise_t *)arg;
uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master();
if (dev) {
data->ret = dev->advertise(data->meta, data->is_advertiser, data->instance);
} else {
data->ret = PX4_ERROR;
}
}
break;
case ORBIOCDEVUNADVERTISE: {
orbiocdevunadvertise_t *data = (orbiocdevunadvertise_t *)arg;
data->ret = uORB::Manager::orb_unadvertise(data->handle);
}
break;
case ORBIOCDEVPUBLISH: {
orbiocdevpublish_t *data = (orbiocdevpublish_t *)arg;
data->ret = uORB::Manager::orb_publish(data->meta, data->handle, data->data);
}
break;
case ORBIOCDEVADDSUBSCRIBER: {
orbiocdevaddsubscriber_t *data = (orbiocdevaddsubscriber_t *)arg;
data->handle = uORB::Manager::orb_add_internal_subscriber(data->orb_id, data->instance, data->initial_generation);
}
break;
case ORBIOCDEVREMSUBSCRIBER: {
uORB::Manager::orb_remove_internal_subscriber(reinterpret_cast<void *>(arg));
}
break;
case ORBIOCDEVQUEUESIZE: {
orbiocdevqueuesize_t *data = (orbiocdevqueuesize_t *)arg;
data->size = uORB::Manager::orb_get_queue_size(data->handle);
}
break;
case ORBIOCDEVDATACOPY: {
orbiocdevdatacopy_t *data = (orbiocdevdatacopy_t *)arg;
data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation);
}
break;
case ORBIOCDEVREGCALLBACK: {
orbiocdevregcallback_t *data = (orbiocdevregcallback_t *)arg;
data->registered = uORB::Manager::register_callback(data->handle, data->callback_sub);
}
break;
case ORBIOCDEVUNREGCALLBACK: {
orbiocdevunregcallback_t *data = (orbiocdevunregcallback_t *)arg;
uORB::Manager::unregister_callback(data->handle, data->callback_sub);
}
break;
case ORBIOCDEVGETINSTANCE: {
orbiocdevgetinstance_t *data = (orbiocdevgetinstance_t *)arg;
data->instance = uORB::Manager::orb_get_instance(data->handle);
}
break;
case ORBIOCDEVMASTERCMD: {
uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master();
if (dev) {
if (arg == ORB_DEVMASTER_TOP) {
dev->showTop(nullptr, 0);
} else {
dev->printStatistics();
}
}
}
break;
case ORBIOCDEVUPDATESAVAIL: {
orbiocdevupdatesavail_t *data = (orbiocdevupdatesavail_t *)arg;
data->ret = updates_available(data->handle, data->last_generation);
}
break;
case ORBIOCDEVISADVERTISED: {
orbiocdevisadvertised_t *data = (orbiocdevisadvertised_t *)arg;
data->ret = is_advertised(data->handle);
}
break;
default:
ret = -ENOTTY;
}
return ret;
}
#endif
int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
{
int ret = PX4_ERROR;
@@ -380,6 +505,19 @@ uint8_t uORB::Manager::orb_get_instance(const void *node_handle)
return -1;
}
/* These are optimized by inlining in NuttX Flat build */
#if !defined(CONFIG_BUILD_FLAT)
unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation)
{
return static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(last_generation);
}
bool uORB::Manager::is_advertised(const void *node_handle)
{
return static_cast<const uORB::DeviceNode *>(node_handle)->is_advertised();
}
#endif
int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance)
{
char path[orb_maxpath];