diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 712e0dd63d..c76548381b 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(_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(_name), nullptr, _interval) +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _interval) /* Parameter handle datatype */ #include diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 8879148a39..1608868101 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -178,6 +178,26 @@ public: return (Subscriber *)sub_px4; } + /** + * 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 + Subscriber *subscribe(const struct orb_metadata *meta, + unsigned interval) + { + SubscriberUORB *sub_px4 = new SubscriberUORB(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 *)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 7d8463cb52..5d4e67ad01 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 *list) : + Subscriber(), + uORB::Subscription(meta, interval, list), + _callback(nullptr) + //XXX store callback + {} + ~SubscriberUORB() {}; /**