mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 16:03:33 +08:00
uORBManager: astyle
This commit is contained in:
Ban Siesta
+315
-315
@@ -49,7 +49,7 @@
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
class Manager;
|
||||
class Manager;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -59,344 +59,344 @@ namespace uORB
|
||||
*/
|
||||
class uORB::Manager : public uORBCommunicator::IChannelRxHandler
|
||||
{
|
||||
public:
|
||||
// public interfaces for this class.
|
||||
public:
|
||||
// public interfaces for this class.
|
||||
|
||||
/**
|
||||
* Method to get the singleton instance for the uORB::Manager.
|
||||
* @return uORB::Manager*
|
||||
*/
|
||||
static uORB::Manager* get_instance();
|
||||
/**
|
||||
* Method to get the singleton instance for the uORB::Manager.
|
||||
* @return uORB::Manager*
|
||||
*/
|
||||
static uORB::Manager *get_instance();
|
||||
|
||||
// ==== uORB interface methods ====
|
||||
/**
|
||||
* Advertise as the publisher of a topic.
|
||||
*
|
||||
* This performs the initial advertisement of a topic; it creates the topic
|
||||
* node in /obj if required and publishes the initial data.
|
||||
*
|
||||
* Any number of advertisers may publish to a topic; publications are atomic
|
||||
* but co-ordination between publishers is not provided by the ORB.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param data A pointer to the initial data to be published.
|
||||
* For topics updated by interrupt handlers, the advertisement
|
||||
* must be performed from non-interrupt context.
|
||||
* @return nullptr on error, otherwise returns an object pointer
|
||||
* that can be used to publish to the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE with no corresponding ORB_DECLARE)
|
||||
* this function will return nullptr and set errno to ENOENT.
|
||||
*/
|
||||
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data);
|
||||
// ==== uORB interface methods ====
|
||||
/**
|
||||
* Advertise as the publisher of a topic.
|
||||
*
|
||||
* This performs the initial advertisement of a topic; it creates the topic
|
||||
* node in /obj if required and publishes the initial data.
|
||||
*
|
||||
* Any number of advertisers may publish to a topic; publications are atomic
|
||||
* but co-ordination between publishers is not provided by the ORB.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param data A pointer to the initial data to be published.
|
||||
* For topics updated by interrupt handlers, the advertisement
|
||||
* must be performed from non-interrupt context.
|
||||
* @return nullptr on error, otherwise returns an object pointer
|
||||
* that can be used to publish to the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE with no corresponding ORB_DECLARE)
|
||||
* this function will return nullptr and set errno to ENOENT.
|
||||
*/
|
||||
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data);
|
||||
|
||||
/**
|
||||
* Advertise as the publisher of a topic.
|
||||
*
|
||||
* This performs the initial advertisement of a topic; it creates the topic
|
||||
* node in /obj if required and publishes the initial data.
|
||||
*
|
||||
* Any number of advertisers may publish to a topic; publications are atomic
|
||||
* but co-ordination between publishers is not provided by the ORB.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param data A pointer to the initial data to be published.
|
||||
* For topics updated by interrupt handlers, the advertisement
|
||||
* must be performed from non-interrupt context.
|
||||
* @param instance Pointer to an integer which will yield the instance ID (0-based)
|
||||
* of the publication.
|
||||
* @param priority The priority of the instance. If a subscriber subscribes multiple
|
||||
* instances, the priority allows the subscriber to prioritize the best
|
||||
* data source as long as its available.
|
||||
* @return ERROR on error, otherwise returns a handle
|
||||
* that can be used to publish to the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE with no corresponding ORB_DECLARE)
|
||||
* this function will return -1 and set errno to ENOENT.
|
||||
*/
|
||||
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
|
||||
int priority) ;
|
||||
/**
|
||||
* Advertise as the publisher of a topic.
|
||||
*
|
||||
* This performs the initial advertisement of a topic; it creates the topic
|
||||
* node in /obj if required and publishes the initial data.
|
||||
*
|
||||
* Any number of advertisers may publish to a topic; publications are atomic
|
||||
* but co-ordination between publishers is not provided by the ORB.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param data A pointer to the initial data to be published.
|
||||
* For topics updated by interrupt handlers, the advertisement
|
||||
* must be performed from non-interrupt context.
|
||||
* @param instance Pointer to an integer which will yield the instance ID (0-based)
|
||||
* of the publication.
|
||||
* @param priority The priority of the instance. If a subscriber subscribes multiple
|
||||
* instances, the priority allows the subscriber to prioritize the best
|
||||
* data source as long as its available.
|
||||
* @return ERROR on error, otherwise returns a handle
|
||||
* that can be used to publish to the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE with no corresponding ORB_DECLARE)
|
||||
* this function will return -1 and set errno to ENOENT.
|
||||
*/
|
||||
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
|
||||
int priority) ;
|
||||
|
||||
|
||||
/**
|
||||
* Publish new data to a topic.
|
||||
*
|
||||
* The data is atomically published to the topic and any waiting subscribers
|
||||
* will be notified. Subscribers that are not waiting can check the topic
|
||||
* for updates using orb_check and/or orb_stat.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @handle The handle returned from orb_advertise.
|
||||
* @param data A pointer to the data to be published.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) ;
|
||||
/**
|
||||
* Publish new data to a topic.
|
||||
*
|
||||
* The data is atomically published to the topic and any waiting subscribers
|
||||
* will be notified. Subscribers that are not waiting can check the topic
|
||||
* for updates using orb_check and/or orb_stat.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @handle The handle returned from orb_advertise.
|
||||
* @param data A pointer to the data to be published.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) ;
|
||||
|
||||
/**
|
||||
* Subscribe to a topic.
|
||||
*
|
||||
* The returned value is a file descriptor that can be passed to poll()
|
||||
* in order to wait for updates to a topic, as well as topic_read,
|
||||
* orb_check and orb_stat.
|
||||
*
|
||||
* Subscription will succeed even if the topic has not been advertised;
|
||||
* in this case the topic will have a timestamp of zero, it will never
|
||||
* signal a poll() event, checking will always return false and it cannot
|
||||
* be copied. When the topic is subsequently advertised, poll, check,
|
||||
* stat and copy calls will react to the initial publication that is
|
||||
* performed as part of the advertisement.
|
||||
*
|
||||
* Subscription will fail if the topic is not known to the system, i.e.
|
||||
* there is nothing in the system that has declared the topic and thus it
|
||||
* can never be published.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @return ERROR on error, otherwise returns a handle
|
||||
* that can be used to read and update the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
|
||||
* this function will return -1 and set errno to ENOENT.
|
||||
*/
|
||||
int orb_subscribe(const struct orb_metadata *meta) ;
|
||||
/**
|
||||
* Subscribe to a topic.
|
||||
*
|
||||
* The returned value is a file descriptor that can be passed to poll()
|
||||
* in order to wait for updates to a topic, as well as topic_read,
|
||||
* orb_check and orb_stat.
|
||||
*
|
||||
* Subscription will succeed even if the topic has not been advertised;
|
||||
* in this case the topic will have a timestamp of zero, it will never
|
||||
* signal a poll() event, checking will always return false and it cannot
|
||||
* be copied. When the topic is subsequently advertised, poll, check,
|
||||
* stat and copy calls will react to the initial publication that is
|
||||
* performed as part of the advertisement.
|
||||
*
|
||||
* Subscription will fail if the topic is not known to the system, i.e.
|
||||
* there is nothing in the system that has declared the topic and thus it
|
||||
* can never be published.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @return ERROR on error, otherwise returns a handle
|
||||
* that can be used to read and update the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
|
||||
* this function will return -1 and set errno to ENOENT.
|
||||
*/
|
||||
int orb_subscribe(const struct orb_metadata *meta) ;
|
||||
|
||||
/**
|
||||
* Subscribe to a multi-instance of a topic.
|
||||
*
|
||||
* The returned value is a file descriptor that can be passed to poll()
|
||||
* in order to wait for updates to a topic, as well as topic_read,
|
||||
* orb_check and orb_stat.
|
||||
*
|
||||
* Subscription will succeed even if the topic has not been advertised;
|
||||
* in this case the topic will have a timestamp of zero, it will never
|
||||
* signal a poll() event, checking will always return false and it cannot
|
||||
* be copied. When the topic is subsequently advertised, poll, check,
|
||||
* stat and copy calls will react to the initial publication that is
|
||||
* performed as part of the advertisement.
|
||||
*
|
||||
* Subscription will fail if the topic is not known to the system, i.e.
|
||||
* there is nothing in the system that has declared the topic and thus it
|
||||
* can never be published.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param instance The instance of the topic. Instance 0 matches the
|
||||
* topic of the orb_subscribe() call, higher indices
|
||||
* are for topics created with orb_publish_multi().
|
||||
* @return ERROR on error, otherwise returns a handle
|
||||
* that can be used to read and update the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
|
||||
* this function will return -1 and set errno to ENOENT.
|
||||
*/
|
||||
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) ;
|
||||
/**
|
||||
* Subscribe to a multi-instance of a topic.
|
||||
*
|
||||
* The returned value is a file descriptor that can be passed to poll()
|
||||
* in order to wait for updates to a topic, as well as topic_read,
|
||||
* orb_check and orb_stat.
|
||||
*
|
||||
* Subscription will succeed even if the topic has not been advertised;
|
||||
* in this case the topic will have a timestamp of zero, it will never
|
||||
* signal a poll() event, checking will always return false and it cannot
|
||||
* be copied. When the topic is subsequently advertised, poll, check,
|
||||
* stat and copy calls will react to the initial publication that is
|
||||
* performed as part of the advertisement.
|
||||
*
|
||||
* Subscription will fail if the topic is not known to the system, i.e.
|
||||
* there is nothing in the system that has declared the topic and thus it
|
||||
* can never be published.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param instance The instance of the topic. Instance 0 matches the
|
||||
* topic of the orb_subscribe() call, higher indices
|
||||
* are for topics created with orb_publish_multi().
|
||||
* @return ERROR on error, otherwise returns a handle
|
||||
* that can be used to read and update the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
|
||||
* this function will return -1 and set errno to ENOENT.
|
||||
*/
|
||||
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) ;
|
||||
|
||||
/**
|
||||
* Unsubscribe from a topic.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_unsubscribe(int handle) ;
|
||||
/**
|
||||
* Unsubscribe from a topic.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_unsubscribe(int handle) ;
|
||||
|
||||
/**
|
||||
* Fetch data from a topic.
|
||||
*
|
||||
* This is the only operation that will reset the internal marker that
|
||||
* indicates that a topic has been updated for a subscriber. Once poll
|
||||
* or check return indicating that an updaet is available, this call
|
||||
* must be used to update the subscription.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param buffer Pointer to the buffer receiving the data, or NULL
|
||||
* if the caller wants to clear the updated flag without
|
||||
* using the data.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) ;
|
||||
/**
|
||||
* Fetch data from a topic.
|
||||
*
|
||||
* This is the only operation that will reset the internal marker that
|
||||
* indicates that a topic has been updated for a subscriber. Once poll
|
||||
* or check return indicating that an updaet is available, this call
|
||||
* must be used to update the subscription.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param buffer Pointer to the buffer receiving the data, or NULL
|
||||
* if the caller wants to clear the updated flag without
|
||||
* using the data.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) ;
|
||||
|
||||
/**
|
||||
* Check whether a topic has been published to since the last orb_copy.
|
||||
*
|
||||
* This check can be used to determine whether to copy the topic when
|
||||
* not using poll(), or to avoid the overhead of calling poll() when the
|
||||
* topic is likely to have updated.
|
||||
*
|
||||
* Updates are tracked on a per-handle basis; this call will continue to
|
||||
* return true until orb_copy is called using the same handle. This interface
|
||||
* should be preferred over calling orb_stat due to the race window between
|
||||
* stat and copy that can lead to missed updates.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param updated Set to true if the topic has been updated since the
|
||||
* last time it was copied using this handle.
|
||||
* @return OK if the check was successful, ERROR otherwise with
|
||||
* errno set accordingly.
|
||||
*/
|
||||
int orb_check(int handle, bool *updated) ;
|
||||
/**
|
||||
* Check whether a topic has been published to since the last orb_copy.
|
||||
*
|
||||
* This check can be used to determine whether to copy the topic when
|
||||
* not using poll(), or to avoid the overhead of calling poll() when the
|
||||
* topic is likely to have updated.
|
||||
*
|
||||
* Updates are tracked on a per-handle basis; this call will continue to
|
||||
* return true until orb_copy is called using the same handle. This interface
|
||||
* should be preferred over calling orb_stat due to the race window between
|
||||
* stat and copy that can lead to missed updates.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param updated Set to true if the topic has been updated since the
|
||||
* last time it was copied using this handle.
|
||||
* @return OK if the check was successful, ERROR otherwise with
|
||||
* errno set accordingly.
|
||||
*/
|
||||
int orb_check(int handle, bool *updated) ;
|
||||
|
||||
/**
|
||||
* Return the last time that the topic was updated.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param time Returns the absolute time that the topic was updated, or zero if it has
|
||||
* never been updated. Time is measured in microseconds.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_stat(int handle, uint64_t *time) ;
|
||||
/**
|
||||
* Return the last time that the topic was updated.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param time Returns the absolute time that the topic was updated, or zero if it has
|
||||
* never been updated. Time is measured in microseconds.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_stat(int handle, uint64_t *time) ;
|
||||
|
||||
/**
|
||||
* Check if a topic has already been created.
|
||||
*
|
||||
* @param meta ORB topic metadata.
|
||||
* @param instance ORB instance
|
||||
* @return OK if the topic exists, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_exists(const struct orb_metadata *meta, int instance) ;
|
||||
/**
|
||||
* Check if a topic has already been created.
|
||||
*
|
||||
* @param meta ORB topic metadata.
|
||||
* @param instance ORB instance
|
||||
* @return OK if the topic exists, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_exists(const struct orb_metadata *meta, int instance) ;
|
||||
|
||||
/**
|
||||
* Return the priority of the topic
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param priority Returns the priority of this topic. This is only relevant for
|
||||
* topics which are published by multiple publishers (e.g. mag0, mag1, etc.)
|
||||
* and allows a subscriber to automatically pick the topic with the highest
|
||||
* priority, independent of the startup order of the associated publishers.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_priority(int handle, int *priority) ;
|
||||
/**
|
||||
* Return the priority of the topic
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param priority Returns the priority of this topic. This is only relevant for
|
||||
* topics which are published by multiple publishers (e.g. mag0, mag1, etc.)
|
||||
* and allows a subscriber to automatically pick the topic with the highest
|
||||
* priority, independent of the startup order of the associated publishers.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_priority(int handle, int *priority) ;
|
||||
|
||||
/**
|
||||
* Set the minimum interval between which updates are seen for a subscription.
|
||||
*
|
||||
* If this interval is set, the subscriber will not see more than one update
|
||||
* within the period.
|
||||
*
|
||||
* Specifically, the first time an update is reported to the subscriber a timer
|
||||
* is started. The update will continue to be reported via poll and orb_check, but
|
||||
* once fetched via orb_copy another update will not be reported until the timer
|
||||
* expires.
|
||||
*
|
||||
* This feature can be used to pace a subscriber that is watching a topic that
|
||||
* would otherwise update too quickly.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param interval An interval period in milliseconds.
|
||||
* @return OK on success, ERROR otherwise with ERRNO set accordingly.
|
||||
*/
|
||||
int orb_set_interval(int handle, unsigned interval) ;
|
||||
/**
|
||||
* Set the minimum interval between which updates are seen for a subscription.
|
||||
*
|
||||
* If this interval is set, the subscriber will not see more than one update
|
||||
* within the period.
|
||||
*
|
||||
* Specifically, the first time an update is reported to the subscriber a timer
|
||||
* is started. The update will continue to be reported via poll and orb_check, but
|
||||
* once fetched via orb_copy another update will not be reported until the timer
|
||||
* expires.
|
||||
*
|
||||
* This feature can be used to pace a subscriber that is watching a topic that
|
||||
* would otherwise update too quickly.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param interval An interval period in milliseconds.
|
||||
* @return OK on success, ERROR otherwise with ERRNO set accordingly.
|
||||
*/
|
||||
int orb_set_interval(int handle, unsigned interval) ;
|
||||
|
||||
/**
|
||||
* Method to set the uORBCommunicator::IChannel instance.
|
||||
* @param comm_channel
|
||||
* The IChannel instance to talk to remote proxies.
|
||||
* @note:
|
||||
* Currently this call only supports the use of one IChannel
|
||||
* Future extensions may include more than one IChannel's.
|
||||
*/
|
||||
void set_uorb_communicator(uORBCommunicator::IChannel* comm_channel);
|
||||
/**
|
||||
* Method to set the uORBCommunicator::IChannel instance.
|
||||
* @param comm_channel
|
||||
* The IChannel instance to talk to remote proxies.
|
||||
* @note:
|
||||
* Currently this call only supports the use of one IChannel
|
||||
* Future extensions may include more than one IChannel's.
|
||||
*/
|
||||
void set_uorb_communicator(uORBCommunicator::IChannel *comm_channel);
|
||||
|
||||
/**
|
||||
* Gets the uORB Communicator instance.
|
||||
*/
|
||||
uORBCommunicator::IChannel* get_uorb_communicator( void );
|
||||
/**
|
||||
* Gets the uORB Communicator instance.
|
||||
*/
|
||||
uORBCommunicator::IChannel *get_uorb_communicator(void);
|
||||
|
||||
/**
|
||||
* Utility method to check if there is a remote subscriber present
|
||||
* for a given topic
|
||||
*/
|
||||
bool is_remote_subscriber_present( const char * messageName );
|
||||
/**
|
||||
* Utility method to check if there is a remote subscriber present
|
||||
* for a given topic
|
||||
*/
|
||||
bool is_remote_subscriber_present(const char *messageName);
|
||||
|
||||
private: // class methods
|
||||
/**
|
||||
* Advertise a node; don't consider it an error if the node has
|
||||
* already been advertised.
|
||||
*
|
||||
* @todo verify that the existing node is the same as the one
|
||||
* we tried to advertise.
|
||||
*/
|
||||
int
|
||||
node_advertise
|
||||
(
|
||||
const struct orb_metadata *meta,
|
||||
int *instance = nullptr,
|
||||
int priority = ORB_PRIO_DEFAULT
|
||||
);
|
||||
private: // class methods
|
||||
/**
|
||||
* Advertise a node; don't consider it an error if the node has
|
||||
* already been advertised.
|
||||
*
|
||||
* @todo verify that the existing node is the same as the one
|
||||
* we tried to advertise.
|
||||
*/
|
||||
int
|
||||
node_advertise
|
||||
(
|
||||
const struct orb_metadata *meta,
|
||||
int *instance = nullptr,
|
||||
int priority = ORB_PRIO_DEFAULT
|
||||
);
|
||||
|
||||
/**
|
||||
* Common implementation for orb_advertise and orb_subscribe.
|
||||
*
|
||||
* Handles creation of the object and the initial publication for
|
||||
* advertisers.
|
||||
*/
|
||||
int
|
||||
node_open
|
||||
(
|
||||
Flavor f,
|
||||
const struct orb_metadata *meta,
|
||||
const void *data,
|
||||
bool advertiser,
|
||||
int *instance = nullptr,
|
||||
int priority = ORB_PRIO_DEFAULT
|
||||
);
|
||||
/**
|
||||
* Common implementation for orb_advertise and orb_subscribe.
|
||||
*
|
||||
* Handles creation of the object and the initial publication for
|
||||
* advertisers.
|
||||
*/
|
||||
int
|
||||
node_open
|
||||
(
|
||||
Flavor f,
|
||||
const struct orb_metadata *meta,
|
||||
const void *data,
|
||||
bool advertiser,
|
||||
int *instance = nullptr,
|
||||
int priority = ORB_PRIO_DEFAULT
|
||||
);
|
||||
|
||||
private: // data members
|
||||
static Manager _Instance;
|
||||
// the communicator channel instance.
|
||||
uORBCommunicator::IChannel* _comm_channel;
|
||||
ORBSet _remote_subscriber_topics;
|
||||
private: // data members
|
||||
static Manager _Instance;
|
||||
// the communicator channel instance.
|
||||
uORBCommunicator::IChannel *_comm_channel;
|
||||
ORBSet _remote_subscriber_topics;
|
||||
|
||||
private: //class methods
|
||||
Manager();
|
||||
private: //class methods
|
||||
Manager();
|
||||
|
||||
/**
|
||||
* Interface to process a received AddSubscription from remote.
|
||||
* @param messageName
|
||||
* This represents the uORB message Name; This message Name should be
|
||||
* globally unique.
|
||||
* @param msgRate
|
||||
* The max rate at which the subscriber can accept the messages.
|
||||
* @return
|
||||
* 0 = success; This means the messages is successfully handled in the
|
||||
* handler.
|
||||
* otherwise = failure.
|
||||
*/
|
||||
virtual int16_t process_add_subscription(const char * messageName,
|
||||
int32_t msgRateInHz);
|
||||
/**
|
||||
* Interface to process a received AddSubscription from remote.
|
||||
* @param messageName
|
||||
* This represents the uORB message Name; This message Name should be
|
||||
* globally unique.
|
||||
* @param msgRate
|
||||
* The max rate at which the subscriber can accept the messages.
|
||||
* @return
|
||||
* 0 = success; This means the messages is successfully handled in the
|
||||
* handler.
|
||||
* otherwise = failure.
|
||||
*/
|
||||
virtual int16_t process_add_subscription(const char *messageName,
|
||||
int32_t msgRateInHz);
|
||||
|
||||
/**
|
||||
* Interface to process a received control msg to remove subscription
|
||||
* @param messageName
|
||||
* This represents the uORB message Name; This message Name should be
|
||||
* globally unique.
|
||||
* @return
|
||||
* 0 = success; This means the messages is successfully handled in the
|
||||
* handler.
|
||||
* otherwise = failure.
|
||||
*/
|
||||
virtual int16_t process_remove_subscription(const char * messageName);
|
||||
/**
|
||||
* Interface to process a received control msg to remove subscription
|
||||
* @param messageName
|
||||
* This represents the uORB message Name; This message Name should be
|
||||
* globally unique.
|
||||
* @return
|
||||
* 0 = success; This means the messages is successfully handled in the
|
||||
* handler.
|
||||
* otherwise = failure.
|
||||
*/
|
||||
virtual int16_t process_remove_subscription(const char *messageName);
|
||||
|
||||
/**
|
||||
* Interface to process the received data message.
|
||||
* @param messageName
|
||||
* This represents the uORB message Name; This message Name should be
|
||||
* globally unique.
|
||||
* @param length
|
||||
* The length of the data buffer to be sent.
|
||||
* @param data
|
||||
* The actual data to be sent.
|
||||
* @return
|
||||
* 0 = success; This means the messages is successfully handled in the
|
||||
* handler.
|
||||
* otherwise = failure.
|
||||
*/
|
||||
virtual int16_t process_received_message(const char * messageName,
|
||||
int32_t length, uint8_t* data);
|
||||
/**
|
||||
* Interface to process the received data message.
|
||||
* @param messageName
|
||||
* This represents the uORB message Name; This message Name should be
|
||||
* globally unique.
|
||||
* @param length
|
||||
* The length of the data buffer to be sent.
|
||||
* @param data
|
||||
* The actual data to be sent.
|
||||
* @return
|
||||
* 0 = success; This means the messages is successfully handled in the
|
||||
* handler.
|
||||
* otherwise = failure.
|
||||
*/
|
||||
virtual int16_t process_received_message(const char *messageName,
|
||||
int32_t length, uint8_t *data);
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -46,337 +46,360 @@ uORB::Manager uORB::Manager::_Instance;
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
uORB::Manager* uORB::Manager::get_instance()
|
||||
uORB::Manager *uORB::Manager::get_instance()
|
||||
{
|
||||
return &_Instance;
|
||||
return &_Instance;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
uORB::Manager::Manager()
|
||||
: _comm_channel( nullptr )
|
||||
: _comm_channel(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
|
||||
{
|
||||
/*
|
||||
* Generate the path to the node and try to open it.
|
||||
*/
|
||||
char path[orb_maxpath];
|
||||
int inst = instance;
|
||||
int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst);
|
||||
/*
|
||||
* Generate the path to the node and try to open it.
|
||||
*/
|
||||
char path[orb_maxpath];
|
||||
int inst = instance;
|
||||
int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst);
|
||||
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return uORB::ERROR;
|
||||
}
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return uORB::ERROR;
|
||||
}
|
||||
|
||||
struct stat buffer;
|
||||
return stat(path, &buffer);
|
||||
struct stat buffer;
|
||||
|
||||
return stat(path, &buffer);
|
||||
}
|
||||
|
||||
orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data)
|
||||
{
|
||||
return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT);
|
||||
return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
|
||||
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
|
||||
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;
|
||||
int result, fd;
|
||||
orb_advert_t advertiser;
|
||||
|
||||
/* open the node as an advertiser */
|
||||
fd = node_open(PUBSUB, meta, data, true, instance, priority);
|
||||
if (fd == ERROR)
|
||||
return nullptr;
|
||||
/* open the node as an advertiser */
|
||||
fd = node_open(PUBSUB, meta, data, true, instance, priority);
|
||||
|
||||
/* get the advertiser handle and close the node */
|
||||
result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
||||
close(fd);
|
||||
if (result == ERROR)
|
||||
return nullptr;
|
||||
if (fd == ERROR) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
/* the advertiser must perform an initial publish to initialise the object */
|
||||
result = orb_publish(meta, advertiser, data);
|
||||
if (result == ERROR)
|
||||
return nullptr;
|
||||
/* get the advertiser handle and close the node */
|
||||
result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
||||
close(fd);
|
||||
|
||||
return advertiser;
|
||||
if (result == ERROR) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
/* the advertiser must perform an initial publish to initialise the object */
|
||||
result = orb_publish(meta, advertiser, data);
|
||||
|
||||
if (result == ERROR) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return advertiser;
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
|
||||
{
|
||||
return node_open(PUBSUB, meta, nullptr, false);
|
||||
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 inst = instance;
|
||||
return node_open(PUBSUB, meta, nullptr, false, &inst);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_unsubscribe(int handle)
|
||||
{
|
||||
return close(handle);
|
||||
return close(handle);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
|
||||
{
|
||||
return uORB::DeviceNode::publish(meta, handle, data);
|
||||
return uORB::DeviceNode::publish(meta, handle, data);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
|
||||
{
|
||||
int ret;
|
||||
int ret;
|
||||
|
||||
ret = read(handle, buffer, meta->o_size);
|
||||
ret = read(handle, buffer, meta->o_size);
|
||||
|
||||
if (ret < 0)
|
||||
return ERROR;
|
||||
if (ret < 0) {
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
if (ret != (int)meta->o_size) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
if (ret != (int)meta->o_size) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
return OK;
|
||||
return OK;
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_check(int handle, bool *updated)
|
||||
{
|
||||
return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
|
||||
return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_stat(int handle, uint64_t *time)
|
||||
{
|
||||
return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time);
|
||||
return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_priority(int handle, int *priority)
|
||||
{
|
||||
return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority);
|
||||
return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_set_interval(int handle, unsigned interval)
|
||||
{
|
||||
return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
|
||||
return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
|
||||
}
|
||||
|
||||
|
||||
int uORB::Manager::node_advertise
|
||||
(
|
||||
const struct orb_metadata *meta,
|
||||
int *instance,
|
||||
int priority
|
||||
const struct orb_metadata *meta,
|
||||
int *instance,
|
||||
int priority
|
||||
)
|
||||
{
|
||||
int fd = -1;
|
||||
int ret = ERROR;
|
||||
int fd = -1;
|
||||
int ret = ERROR;
|
||||
|
||||
/* fill advertiser data */
|
||||
const struct orb_advertdata adv = { meta, instance, priority };
|
||||
/* fill advertiser data */
|
||||
const struct orb_advertdata adv = { meta, instance, priority };
|
||||
|
||||
/* open the control device */
|
||||
fd = open(TOPIC_MASTER_DEVICE_PATH, 0);
|
||||
/* open the control device */
|
||||
fd = open(TOPIC_MASTER_DEVICE_PATH, 0);
|
||||
|
||||
if (fd < 0)
|
||||
goto out;
|
||||
if (fd < 0) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* advertise the object */
|
||||
ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);
|
||||
/* advertise the object */
|
||||
ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);
|
||||
|
||||
/* it's OK if it already exists */
|
||||
if ((OK != ret) && (EEXIST == errno)) {
|
||||
ret = OK;
|
||||
}
|
||||
/* it's OK if it already exists */
|
||||
if ((OK != ret) && (EEXIST == errno)) {
|
||||
ret = OK;
|
||||
}
|
||||
|
||||
out:
|
||||
|
||||
if (fd >= 0)
|
||||
close(fd);
|
||||
if (fd >= 0) {
|
||||
close(fd);
|
||||
}
|
||||
|
||||
return ret;
|
||||
return ret;
|
||||
}
|
||||
|
||||
int uORB::Manager::node_open
|
||||
(
|
||||
Flavor f,
|
||||
const struct orb_metadata *meta,
|
||||
const void *data,
|
||||
bool advertiser,
|
||||
int *instance,
|
||||
int priority
|
||||
Flavor f,
|
||||
const struct orb_metadata *meta,
|
||||
const void *data,
|
||||
bool advertiser,
|
||||
int *instance,
|
||||
int priority
|
||||
)
|
||||
{
|
||||
char path[orb_maxpath];
|
||||
int fd, ret;
|
||||
char path[orb_maxpath];
|
||||
int fd, ret;
|
||||
|
||||
/*
|
||||
* If meta is null, the object was not defined, i.e. it is not
|
||||
* known to the system. We can't advertise/subscribe such a thing.
|
||||
*/
|
||||
if (nullptr == meta) {
|
||||
errno = ENOENT;
|
||||
return ERROR;
|
||||
}
|
||||
/*
|
||||
* If meta is null, the object was not defined, i.e. it is not
|
||||
* known to the system. We can't advertise/subscribe such a thing.
|
||||
*/
|
||||
if (nullptr == meta) {
|
||||
errno = ENOENT;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/*
|
||||
* Advertiser must publish an initial value.
|
||||
*/
|
||||
if (advertiser && (data == nullptr)) {
|
||||
errno = EINVAL;
|
||||
return ERROR;
|
||||
}
|
||||
/*
|
||||
* Advertiser must publish an initial value.
|
||||
*/
|
||||
if (advertiser && (data == nullptr)) {
|
||||
errno = EINVAL;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/*
|
||||
* Generate the path to the node and try to open it.
|
||||
*/
|
||||
ret = uORB::Utils::node_mkpath(path, f, meta, instance);
|
||||
/*
|
||||
* Generate the path to the node and try to open it.
|
||||
*/
|
||||
ret = uORB::Utils::node_mkpath(path, f, meta, instance);
|
||||
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* open the path as either the advertiser or the subscriber */
|
||||
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
|
||||
/* open the path as either the advertiser or the subscriber */
|
||||
fd = open(path, (advertiser) ? O_WRONLY : O_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);
|
||||
/* the node_advertise call will automatically go for the next free entry */
|
||||
fd = -1;
|
||||
}
|
||||
/* 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);
|
||||
/* the node_advertise call will automatically go for the next free entry */
|
||||
fd = -1;
|
||||
}
|
||||
|
||||
/* we may need to advertise the node... */
|
||||
if (fd < 0) {
|
||||
/* we may need to advertise the node... */
|
||||
if (fd < 0) {
|
||||
|
||||
/* try to create the node */
|
||||
ret = node_advertise(meta, instance, priority);
|
||||
/* try to create the node */
|
||||
ret = node_advertise(meta, instance, priority);
|
||||
|
||||
if (ret == OK) {
|
||||
/* update the path, as it might have been updated during the node_advertise call */
|
||||
ret = uORB::Utils::node_mkpath(path, f, meta, instance);
|
||||
if (ret == OK) {
|
||||
/* update the path, as it might have been updated during the node_advertise call */
|
||||
ret = uORB::Utils::node_mkpath(path, f, meta, instance);
|
||||
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
}
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
/* on success, try the open again */
|
||||
if (ret == OK) {
|
||||
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
|
||||
}
|
||||
}
|
||||
/* on success, try the open again */
|
||||
if (ret == OK) {
|
||||
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
|
||||
}
|
||||
}
|
||||
|
||||
if (fd < 0) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
if (fd < 0) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* everything has been OK, we can return the handle now */
|
||||
return fd;
|
||||
/* everything has been OK, we can return the handle now */
|
||||
return fd;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
void uORB::Manager::set_uorb_communicator( uORBCommunicator::IChannel* channel)
|
||||
void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel)
|
||||
{
|
||||
_comm_channel = channel;
|
||||
if (_comm_channel != nullptr) {
|
||||
_comm_channel->register_handler(this);
|
||||
}
|
||||
_comm_channel = channel;
|
||||
|
||||
if (_comm_channel != nullptr) {
|
||||
_comm_channel->register_handler(this);
|
||||
}
|
||||
}
|
||||
|
||||
uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void )
|
||||
uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator(void)
|
||||
{
|
||||
return _comm_channel;
|
||||
return _comm_channel;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
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)
|
||||
{
|
||||
warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s",
|
||||
__LINE__, messageName );
|
||||
int16_t rc = 0;
|
||||
_remote_subscriber_topics.insert( messageName );
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
|
||||
if (ret == OK) {
|
||||
// get the node name.
|
||||
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
|
||||
if ( node == nullptr) {
|
||||
warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet",
|
||||
__LINE__, messageName );
|
||||
}
|
||||
else{
|
||||
// node is present.
|
||||
node->process_add_subscription(msgRateInHz);
|
||||
}
|
||||
} else {
|
||||
rc = -1;
|
||||
}
|
||||
return rc;
|
||||
warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s",
|
||||
__LINE__, messageName);
|
||||
int16_t rc = 0;
|
||||
_remote_subscriber_topics.insert(messageName);
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName);
|
||||
|
||||
if (ret == OK) {
|
||||
// get the node name.
|
||||
uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath);
|
||||
|
||||
if (node == nullptr) {
|
||||
warnx("[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet",
|
||||
__LINE__, messageName);
|
||||
|
||||
} else {
|
||||
// node is present.
|
||||
node->process_add_subscription(msgRateInHz);
|
||||
}
|
||||
|
||||
} else {
|
||||
rc = -1;
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
int16_t uORB::Manager::process_remove_subscription(
|
||||
const char * messageName)
|
||||
const char *messageName)
|
||||
{
|
||||
warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s",
|
||||
__LINE__, messageName );
|
||||
int16_t rc = -1;
|
||||
_remote_subscriber_topics.erase( messageName );
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
|
||||
if (ret == OK) {
|
||||
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
|
||||
// get the node name.
|
||||
if ( node == nullptr) {
|
||||
warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]",
|
||||
__LINE__, messageName);
|
||||
} else {
|
||||
// node is present.
|
||||
node->process_remove_subscription();
|
||||
rc = 0;
|
||||
}
|
||||
}
|
||||
return rc;
|
||||
warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s",
|
||||
__LINE__, messageName);
|
||||
int16_t rc = -1;
|
||||
_remote_subscriber_topics.erase(messageName);
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName);
|
||||
|
||||
if (ret == OK) {
|
||||
uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath);
|
||||
|
||||
// get the node name.
|
||||
if (node == nullptr) {
|
||||
warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]",
|
||||
__LINE__, messageName);
|
||||
|
||||
} else {
|
||||
// node is present.
|
||||
node->process_remove_subscription();
|
||||
rc = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
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)
|
||||
{
|
||||
//warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName );
|
||||
//warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName );
|
||||
|
||||
int16_t rc = -1;
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
|
||||
if (ret == OK) {
|
||||
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
|
||||
// get the node name.
|
||||
if ( node == nullptr) {
|
||||
warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]",
|
||||
__LINE__, messageName, nodepath );
|
||||
int16_t rc = -1;
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName);
|
||||
|
||||
} else {
|
||||
// node is present.
|
||||
node->process_received_message( length, data );
|
||||
rc = 0;
|
||||
}
|
||||
}
|
||||
return rc;
|
||||
if (ret == OK) {
|
||||
uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath);
|
||||
|
||||
// get the node name.
|
||||
if (node == nullptr) {
|
||||
warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]",
|
||||
__LINE__, messageName, nodepath);
|
||||
|
||||
} else {
|
||||
// node is present.
|
||||
node->process_received_message(length, data);
|
||||
rc = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
bool uORB::Manager::is_remote_subscriber_present( const char * messageName )
|
||||
bool uORB::Manager::is_remote_subscriber_present(const char *messageName)
|
||||
{
|
||||
return _remote_subscriber_topics.find( messageName );
|
||||
return _remote_subscriber_topics.find(messageName);
|
||||
}
|
||||
|
||||
@@ -48,349 +48,368 @@ uORB::Manager uORB::Manager::_Instance;
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
uORB::Manager* uORB::Manager::get_instance()
|
||||
uORB::Manager *uORB::Manager::get_instance()
|
||||
{
|
||||
return &_Instance;
|
||||
return &_Instance;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
uORB::Manager::Manager()
|
||||
: _comm_channel( nullptr )
|
||||
: _comm_channel(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
|
||||
{
|
||||
/*
|
||||
* Generate the path to the node and try to open it.
|
||||
*/
|
||||
char path[orb_maxpath];
|
||||
int inst = instance;
|
||||
int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst);
|
||||
/*
|
||||
* Generate the path to the node and try to open it.
|
||||
*/
|
||||
char path[orb_maxpath];
|
||||
int inst = instance;
|
||||
int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst);
|
||||
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
return px4_access(path, F_OK);
|
||||
return px4_access(path, F_OK);
|
||||
}
|
||||
|
||||
orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data)
|
||||
{
|
||||
//warnx("orb_advertise meta = %p", meta);
|
||||
return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT);
|
||||
//warnx("orb_advertise meta = %p", meta);
|
||||
return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
|
||||
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
|
||||
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;
|
||||
int result, fd;
|
||||
orb_advert_t advertiser;
|
||||
|
||||
//warnx("orb_advertise_multi meta = %p\n", meta);
|
||||
//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;
|
||||
}
|
||||
/* open the node as an advertiser */
|
||||
fd = node_open(PUBSUB, meta, data, true, instance, priority);
|
||||
|
||||
/* 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;
|
||||
}
|
||||
if (fd == ERROR) {
|
||||
warnx("node_open as advertiser failed.");
|
||||
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;
|
||||
}
|
||||
/* get the advertiser handle and close the node */
|
||||
result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
||||
px4_close(fd);
|
||||
|
||||
return advertiser;
|
||||
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;
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
|
||||
{
|
||||
return node_open(PUBSUB, meta, nullptr, false);
|
||||
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 inst = instance;
|
||||
return node_open(PUBSUB, meta, nullptr, false, &inst);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_unsubscribe(int fd)
|
||||
{
|
||||
return px4_close(fd);
|
||||
return px4_close(fd);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
|
||||
{
|
||||
return uORB::DeviceNode::publish(meta, handle, data);
|
||||
return uORB::DeviceNode::publish(meta, handle, data);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
|
||||
{
|
||||
int ret;
|
||||
int ret;
|
||||
|
||||
ret = px4_read(handle, buffer, meta->o_size);
|
||||
ret = px4_read(handle, buffer, meta->o_size);
|
||||
|
||||
if (ret < 0)
|
||||
return ERROR;
|
||||
if (ret < 0) {
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
if (ret != (int)meta->o_size) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
if (ret != (int)meta->o_size) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_check(int handle, bool *updated)
|
||||
{
|
||||
return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
|
||||
return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_stat(int handle, uint64_t *time)
|
||||
{
|
||||
return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time);
|
||||
return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_priority(int handle, int *priority)
|
||||
{
|
||||
return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority);
|
||||
return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_set_interval(int handle, unsigned interval)
|
||||
{
|
||||
return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
|
||||
return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
|
||||
}
|
||||
|
||||
|
||||
int uORB::Manager::node_advertise
|
||||
(
|
||||
const struct orb_metadata *meta,
|
||||
int *instance,
|
||||
int priority
|
||||
const struct orb_metadata *meta,
|
||||
int *instance,
|
||||
int priority
|
||||
)
|
||||
{
|
||||
int fd = -1;
|
||||
int ret = ERROR;
|
||||
int fd = -1;
|
||||
int ret = ERROR;
|
||||
|
||||
/* fill advertiser data */
|
||||
const struct orb_advertdata adv = { meta, instance, priority };
|
||||
/* fill advertiser data */
|
||||
const struct orb_advertdata adv = { meta, instance, priority };
|
||||
|
||||
/* open the control device */
|
||||
fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0);
|
||||
/* open the control device */
|
||||
fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0);
|
||||
|
||||
if (fd < 0)
|
||||
goto out;
|
||||
if (fd < 0) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* advertise the object */
|
||||
ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);
|
||||
/* 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;
|
||||
}
|
||||
/* it's PX4_OK if it already exists */
|
||||
if ((PX4_OK != ret) && (EEXIST == errno)) {
|
||||
ret = PX4_OK;
|
||||
}
|
||||
|
||||
out:
|
||||
|
||||
if (fd >= 0)
|
||||
px4_close(fd);
|
||||
if (fd >= 0) {
|
||||
px4_close(fd);
|
||||
}
|
||||
|
||||
return ret;
|
||||
return ret;
|
||||
}
|
||||
|
||||
int uORB::Manager::node_open
|
||||
(
|
||||
Flavor f,
|
||||
const struct orb_metadata *meta,
|
||||
const void *data,
|
||||
bool advertiser,
|
||||
int *instance,
|
||||
int priority
|
||||
Flavor f,
|
||||
const struct orb_metadata *meta,
|
||||
const void *data,
|
||||
bool advertiser,
|
||||
int *instance,
|
||||
int priority
|
||||
)
|
||||
{
|
||||
char path[orb_maxpath];
|
||||
int fd, ret;
|
||||
char path[orb_maxpath];
|
||||
int fd, ret;
|
||||
|
||||
/*
|
||||
* If meta is null, the object was not defined, i.e. it is not
|
||||
* known to the system. We can't advertise/subscribe such a thing.
|
||||
*/
|
||||
if (nullptr == meta) {
|
||||
errno = ENOENT;
|
||||
return ERROR;
|
||||
}
|
||||
/*
|
||||
* If meta is null, the object was not defined, i.e. it is not
|
||||
* known to the system. We can't advertise/subscribe such a thing.
|
||||
*/
|
||||
if (nullptr == meta) {
|
||||
errno = ENOENT;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/*
|
||||
* Advertiser must publish an initial value.
|
||||
*/
|
||||
if (advertiser && (data == nullptr)) {
|
||||
errno = EINVAL;
|
||||
return ERROR;
|
||||
}
|
||||
/*
|
||||
* Advertiser must publish an initial value.
|
||||
*/
|
||||
if (advertiser && (data == nullptr)) {
|
||||
errno = EINVAL;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/*
|
||||
* Generate the path to the node and try to open it.
|
||||
*/
|
||||
/*
|
||||
* 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 = uORB::Utils::node_mkpath(path, f, meta, instance);
|
||||
// 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 = uORB::Utils::node_mkpath(path, f, meta, instance);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
if (ret != PX4_OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* open the path as either the advertiser or the subscriber */
|
||||
fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY);
|
||||
/* open the path as either the advertiser or the subscriber */
|
||||
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 */
|
||||
px4_close(fd);
|
||||
/* 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 */
|
||||
px4_close(fd);
|
||||
|
||||
/* the node_advertise call will automatically go for the next free entry */
|
||||
fd = -1;
|
||||
}
|
||||
/* the node_advertise call will automatically go for the next free entry */
|
||||
fd = -1;
|
||||
}
|
||||
|
||||
/* we may need to advertise the node... */
|
||||
if (fd < 0) {
|
||||
/* we may need to advertise the node... */
|
||||
if (fd < 0) {
|
||||
|
||||
/* try to create the node */
|
||||
ret = node_advertise(meta, instance, priority);
|
||||
/* try to create the node */
|
||||
ret = node_advertise(meta, instance, priority);
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
}
|
||||
if (ret != PX4_OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
/* on success, try the open again */
|
||||
if (ret == PX4_OK) {
|
||||
fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY);
|
||||
}
|
||||
}
|
||||
/* on success, try the open again */
|
||||
if (ret == PX4_OK) {
|
||||
fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY);
|
||||
}
|
||||
}
|
||||
|
||||
if (fd < 0) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
if (fd < 0) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* everything has been OK, we can return the handle now */
|
||||
return fd;
|
||||
/* everything has been OK, we can return the handle now */
|
||||
return fd;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
void uORB::Manager::set_uorb_communicator( uORBCommunicator::IChannel* channel)
|
||||
void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel)
|
||||
{
|
||||
_comm_channel = channel;
|
||||
if (_comm_channel != nullptr) {
|
||||
_comm_channel->register_handler(this);
|
||||
}
|
||||
_comm_channel = channel;
|
||||
|
||||
if (_comm_channel != nullptr) {
|
||||
_comm_channel->register_handler(this);
|
||||
}
|
||||
}
|
||||
|
||||
uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void )
|
||||
uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator(void)
|
||||
{
|
||||
return _comm_channel;
|
||||
return _comm_channel;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
int16_t uORB::Manager::process_add_subscription(const char *messageName,
|
||||
int32_t msgRateInHz)
|
||||
int32_t msgRateInHz)
|
||||
{
|
||||
warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s",
|
||||
__LINE__, messageName );
|
||||
int16_t rc = 0;
|
||||
_remote_subscriber_topics.insert( messageName );
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
|
||||
if (ret == OK) {
|
||||
// get the node name.
|
||||
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
|
||||
if ( node == nullptr) {
|
||||
warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet",
|
||||
__LINE__, messageName );
|
||||
}
|
||||
else{
|
||||
// node is present.
|
||||
node->process_add_subscription(msgRateInHz);
|
||||
}
|
||||
} else {
|
||||
rc = -1;
|
||||
}
|
||||
return rc;
|
||||
warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s",
|
||||
__LINE__, messageName);
|
||||
int16_t rc = 0;
|
||||
_remote_subscriber_topics.insert(messageName);
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName);
|
||||
|
||||
if (ret == OK) {
|
||||
// get the node name.
|
||||
uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath);
|
||||
|
||||
if (node == nullptr) {
|
||||
warnx("[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet",
|
||||
__LINE__, messageName);
|
||||
|
||||
} else {
|
||||
// node is present.
|
||||
node->process_add_subscription(msgRateInHz);
|
||||
}
|
||||
|
||||
} else {
|
||||
rc = -1;
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
int16_t uORB::Manager::process_remove_subscription(
|
||||
const char * messageName)
|
||||
const char *messageName)
|
||||
{
|
||||
warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s",
|
||||
__LINE__, messageName );
|
||||
int16_t rc = -1;
|
||||
_remote_subscriber_topics.erase( messageName );
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
|
||||
if (ret == OK) {
|
||||
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
|
||||
// get the node name.
|
||||
if ( node == nullptr) {
|
||||
warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]",
|
||||
__LINE__, messageName);
|
||||
} else {
|
||||
// node is present.
|
||||
node->process_remove_subscription();
|
||||
rc = 0;
|
||||
}
|
||||
}
|
||||
return rc;
|
||||
warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s",
|
||||
__LINE__, messageName);
|
||||
int16_t rc = -1;
|
||||
_remote_subscriber_topics.erase(messageName);
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName);
|
||||
|
||||
if (ret == OK) {
|
||||
uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath);
|
||||
|
||||
// get the node name.
|
||||
if (node == nullptr) {
|
||||
warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]",
|
||||
__LINE__, messageName);
|
||||
|
||||
} else {
|
||||
// node is present.
|
||||
node->process_remove_subscription();
|
||||
rc = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
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)
|
||||
{
|
||||
//warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName );
|
||||
//warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName );
|
||||
|
||||
int16_t rc = -1;
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
|
||||
if (ret == OK) {
|
||||
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
|
||||
// get the node name.
|
||||
if ( node == nullptr) {
|
||||
warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]",
|
||||
__LINE__, messageName, nodepath );
|
||||
int16_t rc = -1;
|
||||
char nodepath[orb_maxpath];
|
||||
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName);
|
||||
|
||||
} else {
|
||||
// node is present.
|
||||
node->process_received_message( length, data );
|
||||
rc = 0;
|
||||
}
|
||||
}
|
||||
return rc;
|
||||
if (ret == OK) {
|
||||
uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath);
|
||||
|
||||
// get the node name.
|
||||
if (node == nullptr) {
|
||||
warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]",
|
||||
__LINE__, messageName, nodepath);
|
||||
|
||||
} else {
|
||||
// node is present.
|
||||
node->process_received_message(length, data);
|
||||
rc = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
bool uORB::Manager::is_remote_subscriber_present( const char * messageName )
|
||||
bool uORB::Manager::is_remote_subscriber_present(const char *messageName)
|
||||
{
|
||||
return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() );
|
||||
return (_remote_subscriber_topics.find(messageName) != _remote_subscriber_topics.end());
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user