Browse Source

move uorb::subscriptionbase into constructor of subscriber

sbg
Thomas Gubler 10 years ago
parent
commit
2dfd30c25e
  1. 10
      src/platforms/px4_nodehandle.h
  2. 13
      src/platforms/px4_subscriber.h

10
src/platforms/px4_nodehandle.h

@ -190,8 +190,7 @@ public: @@ -190,8 +190,7 @@ public:
template<typename T>
Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval
{
uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class
SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, std::placeholders::_1));
SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, std::placeholders::_1));
/* Check if this is the smallest interval so far and update _sub_min_interval */
if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
@ -210,8 +209,7 @@ public: @@ -210,8 +209,7 @@ public:
template<typename T, typename C>
Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10)
{
uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class
SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, obj, std::placeholders::_1));
SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, obj, std::placeholders::_1));
/* Check if this is the smallest interval so far and update _sub_min_interval */
if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
@ -230,9 +228,7 @@ public: @@ -230,9 +228,7 @@ public:
template<typename T>
Subscriber<T> *subscribe(unsigned interval=10) //XXX interval
{
uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class
SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(uorb_sub, interval);
SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(interval);
/* Check if this is the smallest interval so far and update _sub_min_interval */
if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {

13
src/platforms/px4_subscriber.h

@ -183,12 +183,14 @@ public: @@ -183,12 +183,14 @@ public:
* Construct SubscriberUORB by providing orb meta data without callback
* @param interval Minimal interval between calls to callback
*/
SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) :
SubscriberUORB(unsigned interval) :
SubscriberNode(interval),
_uorb_sub(uorb_sub)
_uorb_sub(new uORB::SubscriptionBase(T::handle(), interval))
{}
virtual ~SubscriberUORB() {};
virtual ~SubscriberUORB() {
delete _uorb_sub;
};
/**
* Update Subscription
@ -233,10 +235,9 @@ public: @@ -233,10 +235,9 @@ public:
* @param cbf Callback, executed on receiving a new message
* @param interval Minimal interval between calls to callback
*/
SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub,
unsigned interval,
SubscriberUORBCallback(unsigned interval,
std::function<void(const T &)> cbf) :
SubscriberUORB<T>(uorb_sub, interval),
SubscriberUORB<T>(interval),
_cbf(cbf)
{}

Loading…
Cancel
Save