WIP, c++11 style callbacks for px4

1 files changed, 11 insertions, 2 deletions

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index b665f3fe7..f473bf629 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -105,10 +105,12 @@ public:
 	~NodeHandle() {};
 
 	template<typename M>
-	Subscriber * subscribe(const struct orb_metadata *meta, void(*fp)(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 Subscriber(meta, interval, fp, &_subs);
+		Subscriber *sub = new SubscriberPX4<M>(meta, interval, callback, &_subs);
 		return sub;
 	}
 
@@ -118,7 +120,14 @@ public:
 		Publisher * pub = new Publisher(meta, &_pubs);
 		return pub;
 	}
+
+	void spinOnce();
+
+	void spin() {
+	 //XXX: call callbacks and do not return until task is terminated
+	}
 private:
+	static const uint16_t kMaxSubscriptions = 100;
 	List<uORB::SubscriptionNode*> _subs;
 	List<uORB::PublicationNode*> _pubs;