@ -36,7 +36,11 @@
@@ -36,7 +36,11 @@
* A lightweight object broker .
*/
# include <px4_config.h>
# include <px4_platform.h>
# include <px4_device.h>
# include <px4_defines.h>
# include <px4_tasks.h>
# include <px4_posix.h>
# include <drivers/device/device.h>
@ -46,16 +50,12 @@
@@ -46,16 +50,12 @@
# include <string.h>
# include <stdlib.h>
# include <fcntl.h>
# include <poll.h>
# include <errno.h>
# include <stdio.h>
# include <math.h>
# include <unistd.h>
# include <systemlib/err.h>
# include <nuttx/arch.h>
# include <nuttx/wqueue.h>
# include <nuttx/clock.h>
# include <systemlib/systemlib.h>
# include <drivers/drv_hrt.h>
@ -123,17 +123,17 @@ public:
@@ -123,17 +123,17 @@ public:
ORBDevNode ( const struct orb_metadata * meta , const char * name , const char * path , int priority ) ;
~ ORBDevNode ( ) ;
virtual int open ( struct file * fil p) ;
virtual int close ( struct file * fil p) ;
virtual ssize_t read ( struct file * fil p, char * buffer , size_t buflen ) ;
virtual ssize_t write ( struct file * fil p, const char * buffer , size_t buflen ) ;
virtual int ioctl ( struct file * fil p, int cmd , unsigned long arg ) ;
virtual int open ( device : : px4_dev_handle_t * handle p) ;
virtual int close ( device : : px4_dev_handle_t * handle p) ;
virtual ssize_t read ( device : : px4_dev_handle_t * handle p, char * buffer , size_t buflen ) ;
virtual ssize_t write ( device : : px4_dev_handle_t * handle p, const char * buffer , size_t buflen ) ;
virtual int ioctl ( device : : px4_dev_handle_t * handle p, int cmd , unsigned long arg ) ;
static ssize_t publish ( const orb_metadata * meta , orb_advert_t handle , const void * data ) ;
protected :
virtual pollevent_t poll_state ( struct file * fil p) ;
virtual void poll_notify_one ( struct pollfd * fds , pollevent_t events ) ;
virtual pollevent_t poll_state ( device : : px4_dev_handle_t * handle p) ;
virtual void poll_notify_one ( px4_p ollfd_struct_t * fds , pollevent_t events ) ;
private :
struct SubscriberData {
@ -152,8 +152,8 @@ private:
@@ -152,8 +152,8 @@ private:
pid_t _publisher ; /**< if nonzero, current publisher */
const int _priority ; /**< priority of topic */
SubscriberData * filp_to_sd ( struct file * fil p) {
SubscriberData * sd = ( SubscriberData * ) ( fil p- > f_ priv) ;
SubscriberData * handlep_to_sd ( device : : px4_dev_handle_t * handle p) {
SubscriberData * sd = ( SubscriberData * ) ( handle p- > priv ) ;
return sd ;
}
@ -199,12 +199,12 @@ ORBDevNode::~ORBDevNode()
@@ -199,12 +199,12 @@ ORBDevNode::~ORBDevNode()
}
int
ORBDevNode : : open ( struct file * fil p)
ORBDevNode : : open ( device : : px4_dev_handle_t * handle p)
{
int ret ;
/* is this a publisher? */
if ( fil p- > f_of lags = = O _WRONLY) {
if ( handle p- > flags = = PX4_F _WRONLY) {
/* become the publisher if we can */
lock ( ) ;
@ -221,7 +221,7 @@ ORBDevNode::open(struct file *filp)
@@ -221,7 +221,7 @@ ORBDevNode::open(struct file *filp)
/* now complete the open */
if ( ret = = OK ) {
ret = CDev : : open ( fil p) ;
ret = CDev : : open ( handle p) ;
/* open failed - not the publisher anymore */
if ( ret ! = OK )
@ -232,7 +232,7 @@ ORBDevNode::open(struct file *filp)
@@ -232,7 +232,7 @@ ORBDevNode::open(struct file *filp)
}
/* is this a new subscriber? */
if ( fil p- > f_of lags = = O _RDONLY) {
if ( handle p- > flags = = PX4_F _RDONLY) {
/* allocate subscriber data */
SubscriberData * sd = new SubscriberData ;
@ -248,9 +248,9 @@ ORBDevNode::open(struct file *filp)
@@ -248,9 +248,9 @@ ORBDevNode::open(struct file *filp)
/* set priority */
sd - > priority = _priority ;
fil p- > f_ priv = ( void * ) sd ;
handle p- > priv = ( void * ) sd ;
ret = CDev : : open ( fil p) ;
ret = CDev : : open ( handle p) ;
if ( ret ! = OK )
delete sd ;
@ -263,14 +263,14 @@ ORBDevNode::open(struct file *filp)
@@ -263,14 +263,14 @@ ORBDevNode::open(struct file *filp)
}
int
ORBDevNode : : close ( struct file * fil p)
ORBDevNode : : close ( device : : px4_dev_handle_t * handle p)
{
/* is this the publisher closing? */
if ( getpid ( ) = = _publisher ) {
_publisher = 0 ;
} else {
SubscriberData * sd = filp_to_sd ( fil p) ;
SubscriberData * sd = handlep_to_sd ( handle p) ;
if ( sd ! = nullptr ) {
hrt_cancel ( & sd - > update_call ) ;
@ -278,13 +278,13 @@ ORBDevNode::close(struct file *filp)
@@ -278,13 +278,13 @@ ORBDevNode::close(struct file *filp)
}
}
return CDev : : close ( fil p) ;
return CDev : : close ( handle p) ;
}
ssize_t
ORBDevNode : : read ( struct file * fil p, char * buffer , size_t buflen )
ORBDevNode : : read ( device : : px4_dev_handle_t * handle p, char * buffer , size_t buflen )
{
SubscriberData * sd = ( SubscriberData * ) filp_to_sd ( fil p) ;
SubscriberData * sd = ( SubscriberData * ) handlep_to_sd ( handle p) ;
/* if the object has not been written yet, return zero */
if ( _data = = nullptr )
@ -321,7 +321,7 @@ ORBDevNode::read(struct file *filp, char *buffer, size_t buflen)
@@ -321,7 +321,7 @@ ORBDevNode::read(struct file *filp, char *buffer, size_t buflen)
}
ssize_t
ORBDevNode : : write ( struct file * fil p, const char * buffer , size_t buflen )
ORBDevNode : : write ( device : : px4_dev_handle_t * handle p, const char * buffer , size_t buflen )
{
/*
* Writes are legal from interrupt context as long as the
@ -330,7 +330,7 @@ ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen)
@@ -330,7 +330,7 @@ ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen)
* Writes outside interrupt context will allocate the object
* if it has not yet been allocated .
*
* Note that fil p will usually be NULL .
* Note that handle p will usually be NULL .
*/
if ( nullptr = = _data ) {
if ( ! up_interrupt_context ( ) ) {
@ -369,9 +369,9 @@ ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen)
@@ -369,9 +369,9 @@ ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen)
}
int
ORBDevNode : : ioctl ( struct file * fil p, int cmd , unsigned long arg )
ORBDevNode : : ioctl ( device : : px4_dev_handle_t * handle p, int cmd , unsigned long arg )
{
SubscriberData * sd = filp_to_sd ( fil p) ;
SubscriberData * sd = handlep_to_sd ( handle p) ;
switch ( cmd ) {
case ORBIOCLASTUPDATE :
@ -396,7 +396,7 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg)
@@ -396,7 +396,7 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg)
default :
/* give it to the superclass */
return CDev : : ioctl ( fil p, cmd , arg ) ;
return CDev : : ioctl ( handle p, cmd , arg ) ;
}
}
@ -427,9 +427,9 @@ ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *d
@@ -427,9 +427,9 @@ ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *d
}
pollevent_t
ORBDevNode : : poll_state ( struct file * fil p)
ORBDevNode : : poll_state ( device : : px4_dev_handle_t * handle p)
{
SubscriberData * sd = filp_to_sd ( fil p) ;
SubscriberData * sd = handlep_to_sd ( handle p) ;
/*
* If the topic appears updated to the subscriber , say so .
@ -441,9 +441,9 @@ ORBDevNode::poll_state(struct file *filp)
@@ -441,9 +441,9 @@ ORBDevNode::poll_state(struct file *filp)
}
void
ORBDevNode : : poll_notify_one ( struct pollfd * fds , pollevent_t events )
ORBDevNode : : poll_notify_one ( px4_p ollfd_struct_t * fds , pollevent_t events )
{
SubscriberData * sd = filp_to_sd ( ( struct file * ) fds - > priv ) ;
SubscriberData * sd = handlep_to_sd ( ( device : : px4_dev_handle_t * ) fds - > priv ) ;
/*
* If the topic looks updated to the subscriber , go ahead and notify them .
@ -560,7 +560,7 @@ public:
@@ -560,7 +560,7 @@ public:
ORBDevMaster ( Flavor f ) ;
~ ORBDevMaster ( ) ;
virtual int ioctl ( struct file * fil p, int cmd , unsigned long arg ) ;
virtual int ioctl ( device : : px4_dev_handle_t * handle p, int cmd , unsigned long arg ) ;
private :
Flavor _flavor ;
} ;
@ -580,7 +580,7 @@ ORBDevMaster::~ORBDevMaster()
@@ -580,7 +580,7 @@ ORBDevMaster::~ORBDevMaster()
}
int
ORBDevMaster : : ioctl ( struct file * fil p, int cmd , unsigned long arg )
ORBDevMaster : : ioctl ( device : : px4_dev_handle_t * handle p, int cmd , unsigned long arg )
{
int ret ;
@ -632,6 +632,7 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
@@ -632,6 +632,7 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
devpath = strdup ( nodepath ) ;
if ( devpath = = nullptr ) {
// FIXME - looks like we leaked memory here for objname
return - ENOMEM ;
}
@ -641,6 +642,8 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
@@ -641,6 +642,8 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
/* if we didn't get a device, that's bad */
if ( node = = nullptr ) {
unlock ( ) ;
// FIXME - looks like we leaked memory here for devpath and objname
return - ENOMEM ;
}
@ -670,7 +673,7 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
@@ -670,7 +673,7 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
default :
/* give it to the superclass */
return CDev : : ioctl ( fil p, cmd , arg ) ;
return CDev : : ioctl ( handle p, cmd , arg ) ;
}
}
@ -745,7 +748,7 @@ int pubsublatency_main(void)
@@ -745,7 +748,7 @@ int pubsublatency_main(void)
float latency_integral = 0.0f ;
/* wakeup source(s) */
struct pollfd fds [ 3 ] ;
px4_p ollfd_struct_t fds [ 3 ] ;
int test_multi_sub = orb_subscribe_multi ( ORB_ID ( orb_test ) , 0 ) ;
int test_multi_sub_medium = orb_subscribe_multi ( ORB_ID ( orb_test_medium ) , 0 ) ;
@ -772,7 +775,7 @@ int pubsublatency_main(void)
@@ -772,7 +775,7 @@ int pubsublatency_main(void)
for ( unsigned i = 0 ; i < maxruns ; i + + ) {
/* wait for up to 500ms for data */
int pret = poll ( & fds [ 0 ] , ( sizeof ( fds ) / sizeof ( fds [ 0 ] ) ) , 500 ) ;
int pret = px4_p oll ( & fds [ 0 ] , ( sizeof ( fds ) / sizeof ( fds [ 0 ] ) ) , 500 ) ;
if ( fds [ 0 ] . revents & POLLIN ) {
orb_copy ( ORB_ID ( orb_test ) , test_multi_sub , & t ) ;
timingsgroup = 0 ;
@ -965,11 +968,11 @@ latency_test(orb_id_t T, bool print)
@@ -965,11 +968,11 @@ latency_test(orb_id_t T, bool print)
/* test pub / sub latency */
int pubsub_task = task_spawn_cmd ( " uorb_latency " ,
int pubsub_task = px4_ task_spawn_cmd( " uorb_latency " ,
SCHED_DEFAULT ,
SCHED_PRIORITY_MAX - 5 ,
1500 ,
( main_t ) & pubsublatency_main ,
( px4_ main_t) & pubsublatency_main ,
nullptr ) ;
/* give the test task some data */
@ -1092,13 +1095,13 @@ node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int pri
@@ -1092,13 +1095,13 @@ node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int pri
const struct orb_advertdata adv = { meta , instance , priority } ;
/* open the control device */
fd = open ( TOPIC_MASTER_DEVICE_PATH , 0 ) ;
fd = px4_ open( TOPIC_MASTER_DEVICE_PATH , 0 ) ;
if ( fd < 0 )
goto out ;
/* advertise the object */
ret = ioctl ( fd , ORBIOCADVERTISE , ( unsigned long ) ( uintptr_t ) & adv ) ;
ret = px4_ ioctl( fd , ORBIOCADVERTISE , ( unsigned long ) ( uintptr_t ) & adv ) ;
/* it's OK if it already exists */
if ( ( OK ! = ret ) & & ( EEXIST = = errno ) ) {
@ -1108,7 +1111,7 @@ node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int pri
@@ -1108,7 +1111,7 @@ node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int pri
out :
if ( fd > = 0 )
close ( fd ) ;
px4_ close( fd ) ;
return ret ;
}
@ -1145,6 +1148,9 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
@@ -1145,6 +1148,9 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
/*
* Generate the path to the node and try to open it .
*/
// FIXME - if *instance is uninitialized, why is this being called? Seems risky and
// its definiately a waste. This is the case in muli-topic test.
ret = node_mkpath ( path , f , meta , instance ) ;
if ( ret ! = OK ) {
@ -1153,12 +1159,13 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
@@ -1153,12 +1159,13 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
}
/* open the path as either the advertiser or the subscriber */
fd = open ( path , ( advertiser ) ? O_WRONLY : O _RDONLY) ;
fd = px4_ open( path , ( advertiser ) ? PX4_F_WRONLY : PX4_F _RDONLY) ;
/* if we want to advertise and the node existed, we have to re-try again */
if ( ( fd > = 0 ) & & ( instance ! = nullptr ) & & ( advertiser ) ) {
/* close the fd, we want a new one */
close ( fd ) ;
px4_close ( fd ) ;
/* the node_advertise call will automatically go for the next free entry */
fd = - 1 ;
}
@ -1181,7 +1188,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
@@ -1181,7 +1188,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
/* on success, try the open again */
if ( ret = = OK ) {
fd = open ( path , ( advertiser ) ? O_WRONLY : O _RDONLY) ;
fd = px4_ open( path , ( advertiser ) ? PX4_F_WRONLY : PX4_F _RDONLY) ;
}
}
@ -1214,8 +1221,8 @@ orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *inst
@@ -1214,8 +1221,8 @@ orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *inst
return ERROR ;
/* get the advertiser handle and close the node */
result = ioctl ( fd , ORBIOCGADVERTISER , ( unsigned long ) & advertiser ) ;
close ( fd ) ;
result = px4_ ioctl( fd , ORBIOCGADVERTISER , ( unsigned long ) & advertiser ) ;
px4_ close( fd ) ;
if ( result = = ERROR )
return ERROR ;
@ -1243,7 +1250,7 @@ orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
@@ -1243,7 +1250,7 @@ orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
int
orb_unsubscribe ( int handle )
{
return close ( handle ) ;
return px4_ close( handle ) ;
}
int
@ -1257,7 +1264,7 @@ orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
@@ -1257,7 +1264,7 @@ orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
{
int ret ;
ret = read ( handle , buffer , meta - > o_size ) ;
ret = px4_ read( handle , ( char * ) buffer , meta - > o_size ) ;
if ( ret < 0 )
return ERROR ;
@ -1273,24 +1280,24 @@ orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
@@ -1273,24 +1280,24 @@ orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
int
orb_check ( int handle , bool * updated )
{
return ioctl ( handle , ORBIOCUPDATED , ( unsigned long ) ( uintptr_t ) updated ) ;
return px4_ ioctl( handle , ORBIOCUPDATED , ( unsigned long ) ( uintptr_t ) updated ) ;
}
int
orb_stat ( int handle , uint64_t * time )
{
return ioctl ( handle , ORBIOCLASTUPDATE , ( unsigned long ) ( uintptr_t ) time ) ;
return px4_ ioctl( handle , ORBIOCLASTUPDATE , ( unsigned long ) ( uintptr_t ) time ) ;
}
int
orb_priority ( int handle , int * priority )
{
return ioctl ( handle , ORBIOCGPRIORITY , ( unsigned long ) ( uintptr_t ) priority ) ;
return px4_ ioctl( handle , ORBIOCGPRIORITY , ( unsigned long ) ( uintptr_t ) priority ) ;
}
int
orb_set_interval ( int handle , unsigned interval )
{
return ioctl ( handle , ORBIOCSETINTERVAL , interval * 1000 ) ;
return px4_ ioctl( handle , ORBIOCSETINTERVAL , interval * 1000 ) ;
}