diff options
-rw-r--r-- | src/platforms/px4_defines.h | 2 | ||||
-rw-r--r-- | src/platforms/px4_nodehandle.h | 20 | ||||
-rw-r--r-- | src/platforms/px4_subscriber.h | 15 |
3 files changed, 36 insertions, 1 deletions
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 712e0dd63..c76548381 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -154,7 +154,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) /* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), nullptr, _interval) +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), _interval) /* Parameter handle datatype */ #include <systemlib/param/param.h> diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 8879148a3..160886810 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -179,6 +179,26 @@ public: } /** + * Subscribe without callback to function + * @param meta Describes the topic which nodehande should subscribe to + * @param interval Minimal interval between data fetches from orb + */ + + template<typename M> + Subscriber<M> *subscribe(const struct orb_metadata *meta, + unsigned interval) + { + SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_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()) { + _sub_min_interval = sub_px4; + } + + return (Subscriber<M> *)sub_px4; + } + + /** * Advertise topic * @param meta Describes the topic which is advertised */ diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 7d8463cb5..5d4e67ad0 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -178,6 +178,21 @@ public: //XXX store callback {} + /** + * Construct SubscriberUORB by providing orb meta data without callback + * @param meta orb metadata for the topic which is used + * @param interval Minimal interval between calls to callback + * @param list subscriber is added to this list + */ + SubscriberUORB(const struct orb_metadata *meta, + unsigned interval, + List<uORB::SubscriptionNode *> *list) : + Subscriber<M>(), + uORB::Subscription<M>(meta, interval, list), + _callback(nullptr) + //XXX store callback + {} + ~SubscriberUORB() {}; /** |