diff options
-rw-r--r-- | src/platforms/px4_nodehandle.h | 2 | ||||
-rw-r--r-- | src/platforms/px4_subscriber.h | 61 |
2 files changed, 41 insertions, 22 deletions
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 160886810..97fb25563 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -168,7 +168,7 @@ public: std::function<void(const M &)> callback, unsigned interval) { - SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, callback, &_subs); + SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(meta, interval, callback, &_subs); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 5d4e67ad0..8c299c748 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -161,46 +161,72 @@ class SubscriberUORB : public uORB::Subscription<M> { public: + /** - * Construct SubscriberUORB by providing orb meta data + * Construct SubscriberUORB by providing orb meta data without callback * @param meta orb metadata for the topic which is used - * @param callback Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ SubscriberUORB(const struct orb_metadata *meta, unsigned interval, - std::function<void(const M &)> callback, List<uORB::SubscriptionNode *> *list) : Subscriber<M>(), - uORB::Subscription<M>(meta, interval, list), - _callback(callback) - //XXX store callback + uORB::Subscription<M>(meta, interval, list) {} + ~SubscriberUORB() {}; + /** - * Construct SubscriberUORB by providing orb meta data without callback + * Update Subscription + * Invoked by the list traversal in NodeHandle::spinOnce + */ + virtual void update() + { + if (!uORB::Subscription<M>::updated()) { + /* Topic not updated */ + return; + } + + /* get latest data */ + uORB::Subscription<M>::update(); + }; + + /* Accessors*/ + /** + * Get the last message value + */ + const M& get() { return uORB::Subscription<M>::getData(); } +}; + +template<typename M> +class SubscriberUORBCallback : + public SubscriberUORB<M> +{ +public: + /** + * Construct SubscriberUORBCallback by providing orb meta data * @param meta orb metadata for the topic which is used + * @param callback Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ - SubscriberUORB(const struct orb_metadata *meta, + SubscriberUORBCallback(const struct orb_metadata *meta, unsigned interval, + std::function<void(const M &)> callback, List<uORB::SubscriptionNode *> *list) : - Subscriber<M>(), - uORB::Subscription<M>(meta, interval, list), - _callback(nullptr) - //XXX store callback + SubscriberUORB<M>(meta, interval, list), + _callback(callback) {} - ~SubscriberUORB() {}; + ~SubscriberUORBCallback() {}; /** * Update Subscription * Invoked by the list traversal in NodeHandle::spinOnce * If new data is available the callback is called */ - void update() + virtual void update() { if (!uORB::Subscription<M>::updated()) { /* Topic not updated, do not call callback */ @@ -221,16 +247,9 @@ public: }; - /* Accessors*/ - /** - * Get the last message value - */ - const M& get() { return uORB::Subscription<M>::getData(); } - protected: std::function<void(const M &)> _callback; /**< Callback handle, called when new data is available */ - }; #endif |