WIP, c++11 style callbacks for px4

6 files changed, 74 insertions, 26 deletions

diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs
index f3ce53b4a..5ae27aa00 100644
--- a/nuttx-configs/px4fmu-v2/nsh/Make.defs
+++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs
@@ -90,7 +90,7 @@ else
     ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
     ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
   else
-    # Linux/Cygwin-native toolchain 
+    # Linux/Cygwin-native toolchain
     MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
     ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
     ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
@@ -117,7 +117,7 @@ ARCHOPTIMIZATION	+= -g
 endif
 
 ARCHCFLAGS		 = -std=gnu99
-ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=c++11
 ARCHWARNINGS		 = -Wall \
 			   -Wextra \
 			   -Wdouble-promotion \
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index 0d0a81d7e..f8b138844 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -36,6 +36,10 @@ void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg)
 {
 	PX4_INFO("I heard: [%lu]", msg.timestamp_last_valid);
 }
+// void rc_channels_callback(int i)
+// {
+	// PX4_INFO("I heard: [%d]", i);
+// }
 namespace px4
 {
 bool task_should_exit = false;
@@ -77,7 +81,11 @@ PX4_MAIN_FUNCTION(subscriber)
 	 * is the number of messages that will be buffered up before beginning to throw
 	 * away the oldest ones.
 	 */
-	n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback);
+	// n.subscribe(PX4_TOPIC(rc_channels), [&](const PX4_TOPIC_T(rc_channels) &msg){rc_channels_callback(msg);});
+	// n.subscribe(PX4_TOPIC(rc_channels), [&](int i){ return rc_channels_callback(i);});
+	// CallbackFunction cbf =  [](int i){ return rc_channels_callback(i);};
+	std::function<void(const PX4_TOPIC_T(rc_channels)&)> cbf =  [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);};
+	n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels), cbf);
 	PX4_INFO("subscribed");
 
 	/**
diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp
index 091b5c6c6..7930a0680 100644
--- a/src/platforms/nuttx/px4_nodehandle.cpp
+++ b/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();
+	}
+}
+
 }
diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp
index 6471e3e38..22daee823 100644
--- a/src/platforms/nuttx/px4_nuttx_impl.cpp
+++ b/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
-
-}
-
 }
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;
 
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 12d422bb3..71c3a766d 100644
--- a/src/platforms/px4_subscriber.h
+++ b/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:
+	Subscriber() {};
+	~Subscriber() {};
+private:
+};
+
+template<typename M>
+class SubscriberPX4 :
+	public Subscriber,
+	public uORB::Subscription<M>
 {
 public:
-	template<typename M>
-	Subscriber(const struct orb_metadata *meta,
+	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