WIP, c++11 style callbacks for px4

2 files changed, 19 insertions, 12 deletions

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
-
-}
-
 }