From 2dfd30c25e2839b762ca127fd4af696284df34b9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 07:17:06 +0100 Subject: [PATCH] move uorb::subscriptionbase into constructor of subscriber --- src/platforms/px4_nodehandle.h | 10 +++------- src/platforms/px4_subscriber.h | 13 +++++++------ 2 files changed, 10 insertions(+), 13 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 40604aa86e..f2d09c15cb 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -190,8 +190,7 @@ public: template Subscriber *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 *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(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: template Subscriber *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 *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, obj, std::placeholders::_1)); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(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: template Subscriber *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 *sub_px4 = new SubscriberUORB(uorb_sub, interval); + SubscriberUORB *sub_px4 = new SubscriberUORB(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) { diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index a54b8eb087..e086cd4c26 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -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: * @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 cbf) : - SubscriberUORB(uorb_sub, interval), + SubscriberUORB(interval), _cbf(cbf) {}