yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

uORB advertise through uORBDeviceMaster directly

Browse Source
This commit is contained in:
Daniel Agar
2018-09-16 22:43:32 -04:00
committed by Beat Küng
parent ad88ef14e1
commit cfac2cc38e
7 changed files with 118 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
src/modules/uORB/uORBManager.cpp
View File

@@ -44,12 +44,8 @@
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
//========================= Static initializations =================
uORB::Manager *uORB::Manager::_Instance = nullptr;
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
bool uORB::Manager::initialize()
{
if (_Instance == nullptr) {
@@ -59,8 +55,6 @@ bool uORB::Manager::initialize()
return _Instance != nullptr;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
uORB::Manager::Manager()
{
#ifdef ORB_USE_PUBLISHER_RULES
@@ -89,17 +83,7 @@ uORB::DeviceMaster *uORB::Manager::get_device_master()
if (!_device_master) {
_device_master = new DeviceMaster();
if (_device_master) {
int ret = _device_master->init();
if (ret != PX4_OK) {
PX4_ERR("Initialization of DeviceMaster failed (%i)", ret);
errno = -ret;
delete _device_master;
_device_master = nullptr;
}
} else {
if (_device_master == nullptr) {
PX4_ERR("Failed to allocate DeviceMaster");
errno = ENOMEM;
}
@@ -188,7 +172,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
#endif /* ORB_USE_PUBLISHER_RULES */
/* open the node as an advertiser */
int fd = node_open(meta, data, true, instance, priority);
int fd = node_open(meta, true, instance, priority);
if (fd == PX4_ERROR) {
PX4_ERR("%s advertise failed", meta->o_name);
@@ -246,13 +230,13 @@ int uORB::Manager::orb_unadvertise(orb_advert_t handle)
int uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
{
return node_open(meta, nullptr, false);
return node_open(meta, false);
}
int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
{
int inst = instance;
return node_open(meta, nullptr, false, &inst);
return node_open(meta, false, &inst);
}
int uORB::Manager::orb_unsubscribe(int fd)
@@ -322,42 +306,27 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
int uORB::Manager::node_advertise(const struct orb_metadata *meta, int *instance, int priority)
{
int fd = -1;
int ret = PX4_ERROR;
/* fill advertiser data */
const struct orb_advertdata adv = { meta, instance, priority };
/* open the control device */
fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0);
if (fd < 0) {
goto out;
if (get_device_master()) {
ret = _device_master->advertise(meta, instance, priority);
}
/* advertise the object */
ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);
/* it's PX4_OK if it already exists */
if ((PX4_OK != ret) && (EEXIST == errno)) {
ret = PX4_OK;
}
out:
if (fd >= 0) {
px4_close(fd);
}
return ret;
}
int uORB::Manager::node_open(const struct orb_metadata *meta, const void *data, bool advertiser, int *instance,
int priority)
int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance, int priority)
{
char path[orb_maxpath];
int fd = -1;
int ret = -1;
int ret = PX4_ERROR;
/*
* If meta is null, the object was not defined, i.e. it is not
@@ -368,14 +337,6 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, const void *data,
return PX4_ERROR;
}
/*
* Advertiser must publish an initial value.
*/
if (advertiser && (data == nullptr)) {
errno = EINVAL;
return PX4_ERROR;
}
/* if we have an instance and are an advertiser, we will generate a new node and set the instance,
* so we do not need to open here */
if (!instance || !advertiser) {