WIP, c++11 style callbacks for px4

src/platforms/nuttx/px4_nodehandle.cpp

@@ -36,9 +36,28 @@
  *
  * PX4 Middleware Wrapper Nodehandle
  */
+#include <px4.h>
 #include <platforms/px4_nodehandle.h>
 
 namespace px4
 {
 bool task_should_exit = false;
+
+
+void NodeHandle::spinOnce() {
+	/* Loop through subscriptions, call callback for updated subscriptions */
+	uORB::SubscriptionNode *sub = _subs.getHead();
+	int count = 0;
+
+	while (sub != nullptr) {
+		if (count++ > kMaxSubscriptions) {
+			PX4_WARN("exceeded max subscriptions");
+			break;
+		}
+
+		sub->update();
+		sub = sub->getSibling();
+	}
+}
+
 }

src/platforms/nuttx/px4_nuttx_impl.cpp

@@ -59,16 +59,4 @@ bool ok()
 	return !task_should_exit;
 }
 
-void spin_once()
-{
-	// XXX check linked list of topics with orb_check() here
-
-}
-
-void spin()
-{
-	// XXX block waiting for updated topics here
-
-}
-
 }

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;
 

src/platforms/px4_subscriber.h

@@ -44,6 +44,7 @@
 /* includes when building for NuttX */
 #include <uORB/Subscription.hpp>
 #include <containers/List.hpp>
+#include <functional>
 #endif
 
 namespace px4
@@ -61,23 +62,46 @@ private:
 	ros::Subscriber _ros_sub;
 };
 #else
-class Subscriber :
-	public uORB::SubscriptionNode
+// typedef std::function<void(int)> CallbackFunction;
+class Subscriber
 {
 public:
-	template<typename M>
-	Subscriber(const struct orb_metadata *meta,
+	Subscriber() {};
+	~Subscriber() {};
+private:
+};
+
+template<typename M>
+class SubscriberPX4 :
+	public Subscriber,
+	public uORB::Subscription<M>
+{
+public:
+	SubscriberPX4(const struct orb_metadata *meta,
 			unsigned interval,
-			void(*fp)(M),
+			std::function<void(const M&)> callback,
+			// std::function<void(int i)> callback,
+			// CallbackFunction callback,
 			List<uORB::SubscriptionNode *> * list) :
-		uORB::SubscriptionNode(meta, interval, list)
+		Subscriber(),
+		uORB::Subscription<M>(meta, interval, list)
 		//XXX store callback
 	{}
-	~Subscriber() {};
+	~SubscriberPX4() {};
 
 	void update() {
-	//XXX list traversal callback, needed?
-	} ;
+		/* get latest data */
+		uORB::Subscription<M>::update();
+
+		/* Call callback which performs actions based on this data */
+		// _callback();
+
+	};
+private:
+	// std::function<void(int i)> _callback;
+	// CallbackFunction _callback;
+	 std::function<void(const M&)> _callback;
+
 };
 #endif