use interval setting correctly, improve px4::spin

3 files changed, 35 insertions, 20 deletions

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index fa863463b..ed0635f18 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -81,16 +81,7 @@ 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), [&](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);
-	// n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels),
-			// [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);});
-	// n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels),
-			// [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);});
-	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback);
+	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 1000);
 	PX4_INFO("subscribed");
 
 	/**
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 79f305140..7efe13bae 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -50,7 +50,7 @@
 #define PX4_INFO ROS_INFO
 #define PX4_TOPIC(_name) #_name
 #define PX4_TOPIC_T(_name) _name
-#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
+#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
 
 #else
 /*
@@ -65,6 +65,6 @@
 #define PX4_INFO warnx
 #define PX4_TOPIC(_name) ORB_ID(_name)
 #define PX4_TOPIC_T(_name) _name##_s
-#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), 	[](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);})
+#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), [](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);}, _interval)
 
 #endif
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index e228dfca5..ced844fd5 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -49,6 +49,7 @@
 #include <inttypes.h>
 #else
 /* includes when building for NuttX */
+#include <poll.h>
 #endif
 
 namespace px4
@@ -99,17 +100,23 @@ class __EXPORT NodeHandle
 public:
 	NodeHandle() :
 		_subs(),
-		_pubs()
+		_pubs(),
+		_sub_min_interval(nullptr)
 	{}
 
 	~NodeHandle() {};
 
 	template<typename M>
-	Subscriber * subscribe(const struct orb_metadata *meta, std::function<void(const M&)> callback) {
-		unsigned interval = 0;//XXX decide how to wrap this, ros equivalent?
-		//XXX
-		Subscriber *sub = new SubscriberPX4<M>(meta, interval, callback, &_subs);
-		return sub;
+	Subscriber * subscribe(const struct orb_metadata *meta,
+			std::function<void(const M&)> callback,
+			unsigned interval) {
+		SubscriberPX4<M> *sub_px4 = new SubscriberPX4<M>(meta, interval, callback, &_subs);
+
+		/* Check if this is the smallest interval so far and update _sub_min_interval */
+		if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() < sub_px4->getInterval()) {
+			_sub_min_interval = sub_px4;
+		}
+		return (Subscriber*)sub_px4;
 	}
 
 	template<typename M>
@@ -122,7 +129,23 @@ public:
 	void spinOnce();
 
 	void spin() {
-		while (true) { //XXX
+		while (true) { //XXX check for termination
+			const int timeout_ms = 100;
+			/* Only continue in the loop if the nodehandle has subscriptions */
+			if (_sub_min_interval == nullptr) {
+				usleep(timeout_ms * 1000);
+				continue;
+			}
+
+			/* Poll fd with smallest interval */
+			struct pollfd pfd;
+			pfd.fd = _sub_min_interval->getHandle();
+			pfd.events = POLLIN;
+			if (poll(&pfd, 1, timeout_ms) <= 0) {
+				/* timed out */
+				continue;
+			}
+
 			spinOnce();
 		}
 	}
@@ -130,7 +153,8 @@ private:
 	static const uint16_t kMaxSubscriptions = 100;
 	List<uORB::SubscriptionNode*> _subs;
 	List<uORB::PublicationNode*> _pubs;
-
+	uORB::SubscriptionNode* _sub_min_interval;	/**< Points to the sub wtih the smallest interval
+							  of all Subscriptions in _subs*/
 };
 #endif
 }