From f4f761042bd4998142ddd3c188c21312908ade35 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sat, 13 Jun 2015 10:30:05 +0100 Subject: [PATCH] uORBManager: astyle --- src/modules/uORB/uORBManager.hpp | 630 ++++++++++++------------- src/modules/uORB/uORBManager_nuttx.cpp | 433 +++++++++-------- src/modules/uORB/uORBManager_posix.cpp | 445 ++++++++--------- 3 files changed, 775 insertions(+), 733 deletions(-) diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index a4fb49c82c..ebe673ba2d 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -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); }; diff --git a/src/modules/uORB/uORBManager_nuttx.cpp b/src/modules/uORB/uORBManager_nuttx.cpp index 7b1bcf33d1..cd6020b09c 100644 --- a/src/modules/uORB/uORBManager_nuttx.cpp +++ b/src/modules/uORB/uORBManager_nuttx.cpp @@ -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); } diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index 7cd0d68e9f..f8b876e629 100644 --- a/src/modules/uORB/uORBManager_posix.cpp +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -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()); }