messagelayer prototype for nuttx

9 files changed, 78 insertions, 74 deletions

diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 4defe49e7..fa2c83262 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -56,7 +56,7 @@ MODULES		+= systemcmds/ver
 # Example modules
 #
 MODULES		+= examples/matlab_csv_serial
-#MODULES		+= examples/subscriber
+MODULES		+= examples/subscriber
 MODULES		+= examples/publisher
 
 #
diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
index c27daed83..826418d11 100644
--- a/msg/templates/msg.h.template
+++ b/msg/templates/msg.h.template
@@ -165,12 +165,13 @@ def get_field_init(field):
 
   return '\n\t%s(%s),'%(field.name, init_value)
 }
-@#ifdef __cplusplus
+#ifdef __cplusplus
 @#class @(spec.short_name)_s {
+struct __EXPORT @(spec.short_name)_s {
 @#public:
-@#else
+#else
 struct @(spec.short_name)_s {
-@#endif
+#endif
 @{
 # loop over all fields and print the type and name
 for field in spec.parsed_fields():
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 1c8f4c62b..c662a07ad 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -43,8 +43,10 @@
 
 using namespace px4;
 
-void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
+// void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) {
+void rc_channels_callback_function(const px4_rc_channels &msg);
+void rc_channels_callback_function(const px4_rc_channels &msg) {
+	PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid);
 }
 
 SubscriberExample::SubscriberExample() :
@@ -63,7 +65,8 @@ SubscriberExample::SubscriberExample() :
 	/* Do some subscriptions */
 	/* Function */
 	// PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
-	_n.subscribe<px4_rc_channels>(rc_channels_callback_function);
+	_n.subscribe<px4_rc_channels>(rc_channels_callback_function); //ROS version
+	// _n.subscribe<px4_rc_channels>(std::bind(&rc_channels_callback_function, std::placeholders::_1)); UORB version
 
 	// [> Class Method <]
 	// PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
@@ -77,8 +80,8 @@ SubscriberExample::SubscriberExample() :
  * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
  * Also the current value of the _sub_rc_chan subscription is printed
  */
-void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
-			msg.timestamp_last_valid,
-			_sub_rc_chan->get().data().timestamp_last_valid);
-}
+// void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
+	// PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
+			// msg.timestamp_last_valid,
+			// _sub_rc_chan->get().data().timestamp_last_valid);
+// }
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index b52ae991b..43dafe010 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -59,7 +59,7 @@ protected:
 	// px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
 	px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
 
-	void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
+	// void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
 
 
 };
diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
index bfca48469..cb1135eb9 100644
--- a/src/platforms/nuttx/px4_messages/px4_rc_channels.h
+++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
@@ -6,7 +6,7 @@
 namespace px4
 {
 
-class  px4_rc_channels :
+class __EXPORT px4_rc_channels :
 	public PX4Message<rc_channels_s>
 {
 public:
diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h
index 05fcf1140..c908f6fc6 100644
--- a/src/platforms/px4_message.h
+++ b/src/platforms/px4_message.h
@@ -49,7 +49,7 @@ namespace px4
 {
 
 template <typename M>
-class PX4Message
+class __EXPORT PX4Message
 {
 	// friend class NodeHandle;
 // #if defined(__PX4_ROS)
@@ -69,6 +69,7 @@ public:
 	virtual ~PX4Message() {};
 
 	virtual M& data() {return _data;}
+	virtual const M& data() const {return _data;}
 	virtual PX4TopicHandle handle() = 0;
 private:
 	M _data;
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index a40581239..2406a4a77 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -89,7 +89,8 @@ public:
 		// return (Subscriber<M> *)sub;
 	// }
 	template<typename T>
-	Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &))
+	// Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &))
+	Subscriber<T> *subscribe(void(*fp)(const T &))
 	{
 		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1));
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault,
@@ -240,17 +241,20 @@ public:
 	 */
 
 	template<typename T>
-	Subscriber<T> *subscribe(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback,  unsigned interval=10) //XXX interval
+	// Subscriber<T> *subscribe(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback,  unsigned interval=10) //XXX interval
+	// Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &),  unsigned interval=10) //XXX interval
+	Subscriber<T> *subscribe(void(*fp)(const T &),  unsigned interval=10) //XXX interval
 	{
 		const struct orb_metadata * meta = NULL;
 		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval);
-		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, callback);
+		// SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, callback);
+		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, std::placeholders::_1));
 
-		/* Check if this is the smallest interval so far and update _sub_min_interval */
-		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
-			_sub_min_interval = sub_px4;
-		}
-		_subs.add((SubscriberNode *)sub_px4);
+		// [> Check if this is the smallest interval so far and update _sub_min_interval <]
+		// if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
+			// _sub_min_interval = sub_px4;
+		// }
+		// _subs.add((SubscriberNode *)sub_px4);
 
 		return (Subscriber<T> *)sub_px4;
 	}
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index aff045d23..6d75e28fc 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -54,7 +54,7 @@ namespace px4
 /**
  * Untemplated publisher base class
  * */
-class PublisherBase
+class __EXPORT PublisherBase
 {
 public:
 	PublisherBase() {};
@@ -99,7 +99,7 @@ private:
 	ros::Publisher _ros_pub;	/**< Handle to the ros publisher */
 };
 #else
-class Publisher :
+class __EXPORT Publisher :
 	// public uORB::PublicationNode,
 	public PublisherBase,
 	public ListNode<Publisher *>
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index ef03922ad..88cc86ab8 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -57,11 +57,11 @@ namespace px4
 /**
  * Untemplated subscriber base class
  * */
-class SubscriberBase
+class __EXPORT SubscriberBase
 {
 public:
 	SubscriberBase() {};
-	~SubscriberBase() {};
+	virtual ~SubscriberBase() {};
 
 };
 
@@ -69,25 +69,25 @@ public:
  * Subscriber class which is used by nodehandle
  */
 template<typename T>
-class Subscriber :
+class __EXPORT Subscriber :
 	public SubscriberBase
 {
 public:
 	Subscriber() :
-		SubscriberBase()
+		SubscriberBase(),
+		_msg_current()
 	{};
-	~Subscriber() {};
+
+	virtual ~Subscriber() {}
 
 	/* Accessors*/
 	/**
 	 * Get the last message value
 	 */
-	virtual T get() = 0;
+	virtual T get() {return _msg_current;}
 
-	/**
-	 * Get void pointer to last message value
-	 */
-	virtual void *get_void_ptr() = 0;
+protected:
+	T _msg_current;				/**< Current Message value */
 };
 
 #if defined(__PX4_ROS)
@@ -104,11 +104,11 @@ public:
 	/**
 	 * Construct Subscriber by providing a callback function
 	 */
-	SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) :
+	// SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) :
+	SubscriberROS(std::function<void(const T &)> cbf) :
 		Subscriber<T>(),
 		_ros_sub(),
-		_cbf(cbf),
-		_msg_current()
+		_cbf(cbf)
 	{}
 
 	/**
@@ -117,35 +117,26 @@ public:
 	SubscriberROS() :
 		Subscriber<T>(),
 		_ros_sub(),
-		_cbf(NULL),
-		_msg_current()
+		_cbf(NULL)
 	{}
 
 
-	~SubscriberROS() {};
-
-	/* Accessors*/
-	/**
-	 * Get the last message value
-	 */
-	T get() { return _msg_current; }
-	/**
-	 * Get void pointer to last message value
-	 */
-	void *get_void_ptr() { return (void *)&_msg_current; }
+	virtual ~SubscriberROS() {};
 
 protected:
 	/**
 	 * Called on topic update, saves the current message and then calls the provided callback function
+	 * needs to use the native type as it is called by ROS
 	 */
 	void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg)
 	{
 		/* Store data */
-		_msg_current = (T)msg;
+		this->_msg_current = (T)msg;
 
 		/* Call callback */
 		if (_cbf != NULL) {
-			_cbf(msg);
+			// _cbf(_msg_current);
+			_cbf(this->get());
 		}
 
 	}
@@ -159,8 +150,7 @@ protected:
 	}
 
 	ros::Subscriber _ros_sub;		/**< Handle to ros subscriber */
-	std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _cbf;	/**< Callback that the user provided on the subscription */
-	T _msg_current;				/**< Current Message value */
+	std::function<void(const T &)> _cbf;	/**< Callback that the user provided on the subscription */
 
 };
 
@@ -170,7 +160,7 @@ protected:
 /**
  * Because we maintain a list of subscribers we need a node class
  */
-class SubscriberNode :
+class __EXPORT SubscriberNode :
 	public ListNode<SubscriberNode *>
 {
 public:
@@ -197,7 +187,7 @@ protected:
  * Subscriber class that is templated with the uorb subscription message type
  */
 template<typename T>
-class SubscriberUORB :
+class __EXPORT SubscriberUORB :
 	public Subscriber<T>,
 	public SubscriberNode
 {
@@ -220,7 +210,7 @@ public:
 		_uorb_sub(uorb_sub)
 	{}
 
-	~SubscriberUORB() {};
+	virtual ~SubscriberUORB() {};
 
 	/**
 	 * Update Subscription
@@ -229,11 +219,10 @@ public:
 	virtual void update()
 	{
 		if (!_uorb_sub->updated()) {
-			/* Topic not updated */
+			/* Topic not updated, do not call callback */
 			return;
 		}
 
-		/* get latest data */
 		_uorb_sub->update(get_void_ptr());
 	};
 
@@ -241,11 +230,16 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	T get() { return (T)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
+	// T get() { return (T)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
+	// T get() {
+		// typename std::remove_reference<decltype(((T*)nullptr)->data())>::type msg = (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub;
+		// return (T)msg;
+	// }
+
 	/**
 	 * Get void pointer to last message value
 	 */
-	void *get_void_ptr() { return (void *)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type*)_uorb_sub; }
+	void *get_void_ptr() { return (void *)&(this->_msg_current.data()); }
 
 	int getUORBHandle() { return _uorb_sub->getHandle(); }
 
@@ -256,7 +250,7 @@ protected:
 
 //XXX reduce to one class with overloaded constructor?
 template<typename T>
-class SubscriberUORBCallback :
+class __EXPORT SubscriberUORBCallback :
 	public SubscriberUORB<T>
 {
 public:
@@ -275,12 +269,13 @@ public:
 		// _callback(callback)
 	// {}
 	SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub,
-			       std::function<void(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback) :
-		SubscriberUORB<T>(uorb_sub),
-		_callback(callback)
+				unsigned interval,
+			       std::function<void(const T &)> cbf) :
+		SubscriberUORB<T>(uorb_sub, interval),
+		_cbf(cbf)
 	{}
 
-	~SubscriberUORBCallback() {};
+	virtual ~SubscriberUORBCallback() {};
 
 	/**
 	 * Update Subscription
@@ -289,28 +284,28 @@ public:
 	 */
 	virtual void update()
 	{
-		if (!SubscriberUORB<T>::_uorb_sub->updated()) {
+		if (!this->_uorb_sub->updated()) {
 			/* Topic not updated, do not call callback */
 			return;
-		}
 
 		/* get latest data */
-		SubscriberUORB<T>::_uorb_sub->update();
+		this->_uorb_sub->update(this->get_void_ptr());
 
 
 		/* Check if there is a callback */
-		if (_callback == nullptr) {
+		if (_cbf == nullptr) {
 			return;
 		}
 
 		/* Call callback which performs actions based on this data */
-		_callback(SubscriberUORB<T>::getUORBData());
+		_cbf(Subscriber<T>::get());
+		}
 
 	};
 
 protected:
-	std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _callback;	/**< Callback handle,
-							  called when new data is available */
+	// std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _callback;	[>*< Callback handle, called when new data is available */
+	std::function<void(const T &)> _cbf;	/**< Callback that the user provided on the subscription */
 };
 #endif