actually call callback

2 files changed, 7 insertions, 10 deletions

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index f473bf629..e228dfca5 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -106,8 +106,6 @@ public:
 
 	template<typename M>
 	Subscriber * subscribe(const struct orb_metadata *meta, std::function<void(const M&)> callback) {
-	// Subscriber * subscribe(const struct orb_metadata *meta, std::function<void(int i)> callback) {
-	// Subscriber * subscribe(const struct orb_metadata *meta, CallbackFunction callback) {
 		unsigned interval = 0;//XXX decide how to wrap this, ros equivalent?
 		//XXX
 		Subscriber *sub = new SubscriberPX4<M>(meta, interval, callback, &_subs);
@@ -124,7 +122,9 @@ public:
 	void spinOnce();
 
 	void spin() {
-	 //XXX: call callbacks and do not return until task is terminated
+		while (true) { //XXX
+			spinOnce();
+		}
 	}
 private:
 	static const uint16_t kMaxSubscriptions = 100;
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 71c3a766d..efec8f2a3 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -80,11 +80,10 @@ public:
 	SubscriberPX4(const struct orb_metadata *meta,
 			unsigned interval,
 			std::function<void(const M&)> callback,
-			// std::function<void(int i)> callback,
-			// CallbackFunction callback,
 			List<uORB::SubscriptionNode *> * list) :
 		Subscriber(),
-		uORB::Subscription<M>(meta, interval, list)
+		uORB::Subscription<M>(meta, interval, list),
+		_callback(callback)
 		//XXX store callback
 	{}
 	~SubscriberPX4() {};
@@ -94,13 +93,11 @@ public:
 		uORB::Subscription<M>::update();
 
 		/* Call callback which performs actions based on this data */
-		// _callback();
+		_callback(uORB::Subscription<M>::getData());
 
 	};
 private:
-	// std::function<void(int i)> _callback;
-	// CallbackFunction _callback;
-	 std::function<void(const M&)> _callback;
+	std::function<void(const M&)> _callback;
 
 };
 #endif