@ -64,9 +64,7 @@ bool uORB::Manager::initialize()
@@ -64,9 +64,7 @@ bool uORB::Manager::initialize()
uORB : : Manager : : Manager ( )
: _comm_channel ( nullptr )
{
for ( int i = 0 ; i < Flavor_count ; + + i ) {
_device_masters [ i ] = nullptr ;
}
_device_master = nullptr ;
# ifdef ORB_USE_PUBLISHER_RULES
const char * file_name = " ./rootfs/orb_publisher.rules " ;
@ -86,26 +84,22 @@ uORB::Manager::Manager()
@@ -86,26 +84,22 @@ uORB::Manager::Manager()
uORB : : Manager : : ~ Manager ( )
{
for ( int i = 0 ; i < Flavor_count ; + + i ) {
if ( _device_masters [ i ] ) {
delete _device_masters [ i ] ;
}
}
delete _device_master ;
}
uORB : : DeviceMaster * uORB : : Manager : : get_device_master ( Flavor flavor )
uORB : : DeviceMaster * uORB : : Manager : : get_device_master ( )
{
if ( ! _device_masters [ flavor ] ) {
_device_masters [ flavor ] = new DeviceMaster ( flavor ) ;
if ( ! _device_master ) {
_device_master = new DeviceMaster ( ) ;
if ( _device_masters [ flavor ] ) {
int ret = _device_masters [ flavor ] - > init ( ) ;
if ( _device_master ) {
int ret = _device_master - > init ( ) ;
if ( ret ! = PX4_OK ) {
PX4_ERR ( " Initialization of DeviceMaster failed (%i) " , ret ) ;
errno = - ret ;
delete _device_masters [ flavor ] ;
_device_masters [ flavor ] = nullptr ;
delete _device_master ;
_device_master = nullptr ;
}
} else {
@ -114,7 +108,7 @@ uORB::DeviceMaster *uORB::Manager::get_device_master(Flavor flavor)
@@ -114,7 +108,7 @@ uORB::DeviceMaster *uORB::Manager::get_device_master(Flavor flavor)
}
}
return _device_masters [ flavor ] ;
return _device_master ;
}
int uORB : : Manager : : orb_exists ( const struct orb_metadata * meta , int instance )
@ -124,11 +118,11 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
@@ -124,11 +118,11 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
*/
char path [ orb_maxpath ] ;
int inst = instance ;
int ret = uORB : : Utils : : node_mkpath ( path , PUBSUB , meta , & inst ) ;
int ret = uORB : : Utils : : node_mkpath ( path , meta , & inst ) ;
if ( ret ! = OK ) {
errno = - ret ;
return ERROR ;
return PX4_ ERROR;
}
# if defined(__PX4_NUTTX)
@ -138,7 +132,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
@@ -138,7 +132,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
ret = px4_access ( path , F_OK ) ;
if ( ret = = - 1 & & meta ! = nullptr & & ! _remote_topics . empty ( ) ) {
ret = ( _remote_topics . find ( meta - > o_name ) ! = _remote_topics . end ( ) ) ? OK : ERROR ;
ret = ( _remote_topics . find ( meta - > o_name ) ! = _remote_topics . end ( ) ) ? OK : PX4_ ERROR;
}
# endif
@ -154,7 +148,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
@@ -154,7 +148,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
if ( px4_ioctl ( fd , ORBIOCISPUBLISHED , ( unsigned long ) & is_published ) = = 0 ) {
if ( ! is_published ) {
ret = ERROR ;
ret = PX4_ ERROR;
}
}
@ -196,9 +190,9 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
@@ -196,9 +190,9 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
orb_advert_t advertiser ;
/* open the node as an advertiser */
fd = node_open ( PUBSUB , meta , data , true , instance , priority ) ;
fd = node_open ( meta , data , true , instance , priority ) ;
if ( fd = = ERROR ) {
if ( fd = = PX4_ ERROR) {
PX4_ERR ( " %s advertise failed " , meta - > o_name ) ;
return nullptr ;
}
@ -216,7 +210,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
@@ -216,7 +210,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
result = px4_ioctl ( fd , ORBIOCGADVERTISER , ( unsigned long ) & advertiser ) ;
px4_close ( fd ) ;
if ( result = = ERROR ) {
if ( result = = PX4_ ERROR) {
PX4_WARN ( " px4_ioctl ORBIOCGADVERTISER failed. fd = %d " , fd ) ;
return nullptr ;
}
@ -227,7 +221,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
@@ -227,7 +221,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
/* the advertiser must perform an initial publish to initialise the object */
result = orb_publish ( meta , advertiser , data ) ;
if ( result = = ERROR ) {
if ( result = = PX4_ ERROR) {
PX4_WARN ( " orb_publish failed " ) ;
return nullptr ;
}
@ -250,13 +244,13 @@ int uORB::Manager::orb_unadvertise(orb_advert_t handle)
@@ -250,13 +244,13 @@ int uORB::Manager::orb_unadvertise(orb_advert_t handle)
int uORB : : Manager : : orb_subscribe ( const struct orb_metadata * meta )
{
return node_open ( PUBSUB , meta , nullptr , false ) ;
return node_open ( 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 ) ;
return node_open ( meta , nullptr , false , & inst ) ;
}
int uORB : : Manager : : orb_unsubscribe ( int fd )
@ -284,12 +278,12 @@ int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *b
@@ -284,12 +278,12 @@ int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *b
ret = px4_read ( handle , buffer , meta - > o_size ) ;
if ( ret < 0 ) {
return ERROR ;
return PX4_ ERROR;
}
if ( ret ! = ( int ) meta - > o_size ) {
errno = EIO ;
return ERROR ;
return PX4_ ERROR;
}
return PX4_OK ;
@ -333,7 +327,7 @@ int uORB::Manager::node_advertise
@@ -333,7 +327,7 @@ int uORB::Manager::node_advertise
)
{
int fd = - 1 ;
int ret = ERROR ;
int ret = PX4_ ERROR;
/* fill advertiser data */
const struct orb_advertdata adv = { meta , instance , priority } ;
@ -362,15 +356,8 @@ out:
@@ -362,15 +356,8 @@ out:
return ret ;
}
int uORB : : Manager : : node_open
(
Flavor f ,
const struct orb_metadata * meta ,
const void * data ,
bool advertiser ,
int * instance ,
int priority
)
int uORB : : Manager : : node_open ( const struct orb_metadata * meta , const void * data , bool advertiser , int * instance ,
int priority )
{
char path [ orb_maxpath ] ;
int fd = - 1 , ret ;
@ -381,7 +368,7 @@ int uORB::Manager::node_open
@@ -381,7 +368,7 @@ int uORB::Manager::node_open
*/
if ( nullptr = = meta ) {
errno = ENOENT ;
return ERROR ;
return PX4_ ERROR;
}
/*
@ -389,7 +376,7 @@ int uORB::Manager::node_open
@@ -389,7 +376,7 @@ int uORB::Manager::node_open
*/
if ( advertiser & & ( data = = nullptr ) ) {
errno = EINVAL ;
return ERROR ;
return PX4_ ERROR;
}
/* if we have an instance and are an advertiser, we will generate a new node and set the instance,
@ -398,11 +385,11 @@ int uORB::Manager::node_open
@@ -398,11 +385,11 @@ int uORB::Manager::node_open
/*
* Generate the path to the node and try to open it .
*/
ret = uORB : : Utils : : node_mkpath ( path , f , meta , instance ) ;
ret = uORB : : Utils : : node_mkpath ( path , meta , instance ) ;
if ( ret ! = OK ) {
errno = - ret ;
return ERROR ;
return PX4_ ERROR;
}
/* open the path as either the advertiser or the subscriber */
@ -420,11 +407,11 @@ int uORB::Manager::node_open
@@ -420,11 +407,11 @@ int uORB::Manager::node_open
if ( ret = = PX4_OK ) {
/* update the path, as it might have been updated during the node_advertise call */
ret = uORB : : Utils : : node_mkpath ( path , f , meta , instance ) ;
ret = uORB : : Utils : : node_mkpath ( path , meta , instance ) ;
if ( ret ! = PX4_OK ) {
errno = - ret ;
return ERROR ;
return PX4_ ERROR;
}
}
@ -447,7 +434,7 @@ int uORB::Manager::node_open
@@ -447,7 +434,7 @@ int uORB::Manager::node_open
if ( fd < 0 ) {
errno = EIO ;
return ERROR ;
return PX4_ ERROR;
}
/* everything has been OK, we can return the handle now */
@ -488,23 +475,21 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name, bool isAdver
@@ -488,23 +475,21 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name, bool isAdver
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB : : Manager : : process_add_subscription ( const char * messageName ,
int32_t msgRateInHz )
int16_t uORB : : Manager : : process_add_subscription ( const char * messageName , int32_t msgRateInHz )
{
PX4_DEBUG ( " [posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s" ,
__LINE__ , messageName ) ;
PX4_DEBUG ( " entering Manager_process_add_subscription: name: %s " , messageName ) ;
int16_t rc = 0 ;
_remote_subscriber_topics . insert ( messageName ) ;
char nodepath [ orb_maxpath ] ;
int ret = uORB : : Utils : : node_mkpath ( nodepath , PUBSUB , messageName ) ;
DeviceMaster * device_master = get_device_master ( PUBSUB ) ;
int ret = uORB : : Utils : : node_mkpath ( nodepath , messageName ) ;
DeviceMaster * device_master = get_device_master ( ) ;
if ( ret = = OK & & device_master ) {
uORB : : DeviceNode * node = device_master - > getDeviceNode ( nodepath ) ;
if ( node = = nullptr ) {
PX4_DEBUG ( " [posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet " ,
__LINE__ , messageName ) ;
PX4_DEBUG ( " DeviceNode(%s) not created yet " , messageName ) ;
} else {
// node is present.
@ -520,14 +505,13 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName,
@@ -520,14 +505,13 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName,
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB : : Manager : : process_remove_subscription (
const char * messageName )
int16_t uORB : : Manager : : process_remove_subscription ( const char * messageName )
{
int16_t rc = - 1 ;
_remote_subscriber_topics . erase ( messageName ) ;
char nodepath [ orb_maxpath ] ;
int ret = uORB : : Utils : : node_mkpath ( nodepath , PUBSUB , messageName ) ;
DeviceMaster * device_master = get_device_master ( PUBSUB ) ;
int ret = uORB : : Utils : : node_mkpath ( nodepath , messageName ) ;
DeviceMaster * device_master = get_device_master ( ) ;
if ( ret = = OK & & device_master ) {
uORB : : DeviceNode * node = device_master - > getDeviceNode ( nodepath ) ;
@ -549,21 +533,19 @@ int16_t uORB::Manager::process_remove_subscription(
@@ -549,21 +533,19 @@ int16_t uORB::Manager::process_remove_subscription(
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB : : Manager : : process_received_message ( const char * messageName ,
int32_t length , uint8_t * data )
int16_t uORB : : Manager : : process_received_message ( const char * messageName , int32_t length , uint8_t * data )
{
int16_t rc = - 1 ;
char nodepath [ orb_maxpath ] ;
int ret = uORB : : Utils : : node_mkpath ( nodepath , PUBSUB , messageName ) ;
DeviceMaster * device_master = get_device_master ( PUBSUB ) ;
int ret = uORB : : Utils : : node_mkpath ( nodepath , messageName ) ;
DeviceMaster * device_master = get_device_master ( ) ;
if ( ret = = OK & & device_master ) {
uORB : : DeviceNode * node = device_master - > getDeviceNode ( nodepath ) ;
// get the node name.
if ( node = = nullptr ) {
PX4_DEBUG ( " [uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s] " ,
__LINE__ , messageName , nodepath ) ;
PX4_DEBUG ( " No existing subscriber found for message: [%s] nodepath:[%s] " , messageName , nodepath ) ;
} else {
// node is present.