orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
{
int result, fd;
orb_advert_t advertiser;
//warnx("orb_advertise_multi meta = %p\n", meta);
/* open the node as an advertiser */
fd = node_open(PUBSUB, meta, data, true, instance, priority);
if (fd == ERROR) {
warnx("node_open as advertiser failed.");
return nullptr;
}
/* get the advertiser handle and close the node */
result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
px4_close(fd);
if (result == ERROR) {
warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd);
return nullptr;
}
/* the advertiser must perform an initial publish to initialise the object */
result = orb_publish(meta, advertiser, data);
if (result == ERROR) {
warnx("orb_publish failed");
return nullptr;
}
return advertiser;
}
serioStuffPtr_t serio_open(const char *tty_name, unsigned baudrate) {
serioStuffPtr_t serio;
speed_t brc;
if(!(brc = serio_get_baud(baudrate))){
debug(DEBUG_UNEXPECTED, "Invalid baud rate: %u\n", baudrate);
return NULL;
}
/* Allocate memory for our struct */
if(!(serio = malloc(sizeof(serioStuff_t))))
return NULL;
/* Zero it */
memset(serio, 0, sizeof(serioStuff_t));
/* Add the magic number */
serio->magic = SERIO_MAGIC;
/* Allocate memory for line */
if(!(serio->line = malloc(SERIO_MAX_LINE))){
free_seriostuff(serio);
return NULL;
}
/* Duplicate path name */
if(!(serio->path = strdup(tty_name))){
free_seriostuff(serio);
return NULL;
}
serio->brc = (unsigned ) brc;
/* Make sure the path is valid */
if(!serio_check_node(serio->path)){
free_seriostuff(serio);
return NULL;
}
/* Open the port */
if(!node_open(serio)){
free_seriostuff(serio);
return NULL;
}
return(serio);
}
orb_advert_t
orb_advertise(const struct orb_metadata *meta, const void *data)
{
int result, fd;
orb_advert_t advertiser;
/* open the node as an advertiser */
fd = node_open(PUBSUB, meta, data, true);
if (fd == ERROR)
return ERROR;
/* get the advertiser handle and close the node */
result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
close(fd);
if (result == ERROR)
return ERROR;
/* the advertiser must perform an initial publish to initialise the object */
result= orb_publish(meta, advertiser, data);
if (result == ERROR)
return ERROR;
return advertiser;
}
int
orb_subscribe(const struct orb_metadata *meta)
{
return node_open(PUBSUB, meta, nullptr, false);
}
int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
{
int inst = instance;
return node_open(PUBSUB, meta, nullptr, false, &inst);
}
int uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
{
return node_open(meta, nullptr, false);
}
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
int priority, unsigned int queue_size)
{
#ifdef ORB_USE_PUBLISHER_RULES
// check publisher rule
if (_has_publisher_rules) {
const char *prog_name = px4_get_taskname();
if (strcmp(_publisher_rule.module_name, prog_name) == 0) {
if (_publisher_rule.ignore_other_topics) {
if (!findTopic(_publisher_rule, meta->o_name)) {
PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
return (orb_advert_t)_Instance;
}
}
} else {
if (findTopic(_publisher_rule, meta->o_name)) {
PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
return (orb_advert_t)_Instance;
}
}
}
#endif /* ORB_USE_PUBLISHER_RULES */
int result, fd;
orb_advert_t advertiser;
/* open the node as an advertiser */
fd = node_open(meta, data, true, instance, priority);
if (fd == PX4_ERROR) {
PX4_ERR("%s advertise failed", meta->o_name);
return nullptr;
}
/* Set the queue size. This must be done before the first publication; thus it fails if
* this is not the first advertiser.
*/
result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
if (result < 0 && queue_size > 1) {
PX4_WARN("orb_advertise_multi: failed to set queue size");
}
/* get the advertiser handle and close the node */
result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
px4_close(fd);
if (result == PX4_ERROR) {
PX4_WARN("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd);
return nullptr;
}
#ifdef ORB_COMMUNICATOR
// For remote systems call over and inform them
uORB::DeviceNode::topic_advertised(meta, priority);
#endif /* ORB_COMMUNICATOR */
/* the advertiser must perform an initial publish to initialise the object */
result = orb_publish(meta, advertiser, data);
if (result == PX4_ERROR) {
PX4_WARN("orb_publish failed");
return nullptr;
}
return advertiser;
}
