Merge branch 'dev_ros' into dev_ros_rossharedlib

6 files changed, 273 insertions, 40 deletions

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 2dd57940d..af7188f32 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -38,36 +38,99 @@
  */
 
 #pragma once
+/* Get the name of the default value fiven the param name */
+#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT
+
+/* Shortcuts to define parameters when the default value is defined according to PX4_PARAM_DEFAULT_VALUE_NAME */
+#define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
+#define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
+
 
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 /*
  * Building for running within the ROS environment
  */
 #define __EXPORT
+#include "ros/ros.h"
+/* Main entry point */
 #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
+
+/* Print/output wrappers */
 #define PX4_WARN ROS_WARN
 #define PX4_INFO ROS_INFO
+
+/* Topic Handle */
 #define PX4_TOPIC(_name) #_name
+
+/* Topic type */
 #define PX4_TOPIC_T(_name) _name
+
+/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
+/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
+
+/* Parameter handle datatype */
+typedef const char *px4_param_t;
+
+/* Helper fucntions to set ROS params, only int and float supported */
+static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value)
+{
+	ros::param::set(name, value);
+	return (px4_param_t)name;
+};
+static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
+{
+	ros::param::set(name, value);
+	return (px4_param_t)name;
+};
+
+/* Initialize a param, in case of ROS the parameter is sent to the parameter server here */
+#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
+
+/* Get value of parameter */
+#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
+
 #else
 /*
  * Building for NuttX
  */
+#include <platforms/px4_includes.h>
+/* Main entry point */
 #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[])
-#define PX4_WARN warnx
+
+/* Print/output wrappers */
 #define PX4_WARN warnx
 #define PX4_INFO warnx
+
+/* Topic Handle */
 #define PX4_TOPIC(_name) ORB_ID(_name)
+
+/* Topic type */
 #define PX4_TOPIC_T(_name) _name##_s
+
+/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
+/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
+
+/* Parameter handle datatype */
+typedef param_t px4_param_t;
+
+/* Initialize a param, get param handle */
+#define PX4_PARAM_INIT(_name) param_find(#_name)
+
+/* Get value of parameter */
+#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
 #endif
 
-/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */
+/* Shortcut for subscribing to topics
+ * Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback
+ */
 #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
 #define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__)
+
+/* shortcut for advertising topics */
 #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name))
 
 /* wrapper for 2d matrices */
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
new file mode 100644
index 000000000..a3b59b766
--- /dev/null
+++ b/src/platforms/px4_includes.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_includes.h
+ *
+ * Includes headers depending on the build target
+ */
+
+#pragma once
+
+#include <stdbool.h>
+
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/*
+ * Building for running within the ROS environment
+ */
+#include "ros/ros.h"
+#include "px4/rc_channels.h"
+
+#else
+/*
+ * Building for NuttX
+ */
+#include <nuttx/config.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_channels.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+
+#endif
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index dece72907..c1465b4b1 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -50,9 +50,15 @@ __EXPORT void init(int argc, char *argv[], const char *process_name);
 __EXPORT uint64_t get_time_micros();
 
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/**
+ * Returns true if the app/task should continue to run
+ */
 bool ok() { return ros::ok(); }
 #else
 extern bool task_should_exit;
+/**
+ * Returns true if the app/task should continue to run
+ */
 bool ok() { return !task_should_exit; }
 #endif
 
@@ -60,8 +66,15 @@ class Rate
 {
 
 public:
+	/**
+	 * Construct the Rate object and set rate
+	 * @param rate_hz rate from which sleep time is calculated in Hz
+	 */
 	explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; }
 
+	/**
+	 * Sleep for 1/rate_hz s
+	 */
 	void sleep() { usleep(sleep_interval); }
 
 private:
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 415351756..5b7247b20 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -55,7 +55,6 @@
 
 namespace px4
 {
-//XXX create abstract base class
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 class NodeHandle :
 	private ros::NodeHandle
@@ -67,42 +66,67 @@ public:
 		_pubs()
 	{}
 
-	~NodeHandle() {
+	~NodeHandle()
+	{
 		//XXX empty lists
 	};
 
-	/* Constructor with callback to function */
+	/**
+	 * Subscribe with callback to function
+	 * @param topic		Name of the topic
+	 * @param fb		Callback, executed on receiving a new message
+	 */
 	template<typename M>
-	Subscriber * subscribe(const char *topic, void(*fp)(M)) {
+	Subscriber *subscribe(const char *topic, void(*fp)(M))
+	{
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
-		Subscriber * sub = new Subscriber(ros_sub);
+		Subscriber *sub = new Subscriber(ros_sub);
 		_subs.push_back(sub);
 		return sub;
 	}
-	/* Constructor with callback to class method */
+
+	/**
+	 * Subscribe with callback to class method
+	 * @param topic		Name of the topic
+	 * @param fb		Callback, executed on receiving a new message
+	 */
 	template<typename M, typename T>
-	Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) {
+	Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj)
+	{
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj);
-		Subscriber * sub = new Subscriber(ros_sub);
+		Subscriber *sub = new Subscriber(ros_sub);
 		_subs.push_back(sub);
 		return sub;
 	}
 
+	/**
+	 * Advertise topic
+	 * @param topic		Name of the topic
+	 */
 	template<typename M>
-	Publisher * advertise(const char *topic) {
+	Publisher *advertise(const char *topic)
+	{
 		ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
 		Publisher *pub =  new Publisher(ros_pub);
 		_pubs.push_back(pub);
 		return pub;
 	}
 
+	/**
+	 * Calls all callback waiting to be called
+	 */
+	void spinOnce() { ros::spinOnce(); }
+
+	/**
+	 * Keeps calling callbacks for incomming messages, returns when module is terminated
+	 */
 	void spin() { ros::spin(); }
 
-	void spinOnce() { ros::spinOnce(); }
+
 private:
-	static const uint32_t kQueueSizeDefault = 1000;
-	std::list<Subscriber*> _subs;
-	std::list<Publisher*> _pubs;
+	static const uint32_t kQueueSizeDefault = 1000;		/**< Size of queue for ROS */
+	std::list<Subscriber *> _subs;				/**< Subcriptions of node */
+	std::list<Publisher *> _pubs;				/**< Publications of node */
 };
 #else
 class __EXPORT NodeHandle
@@ -116,27 +140,45 @@ public:
 
 	~NodeHandle() {};
 
+	/**
+	 * Subscribe with callback to function
+	 * @param meta		Describes the topic which nodehande should subscribe to
+	 * @param callback	Callback, executed on receiving a new message
+	 * @param interval	Minimal interval between calls to callback
+	 */
+
 	template<typename M>
-	Subscriber * subscribe(const struct orb_metadata *meta,
-			std::function<void(const M&)> callback,
-			unsigned interval) {
+	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;
+
+		return (Subscriber *)sub_px4;
 	}
 
+	/**
+	 * Advertise topic
+	 * @param meta		Describes the topic which is advertised
+	 */
 	template<typename M>
-	Publisher * advertise(const struct orb_metadata *meta) {
+	Publisher *advertise(const struct orb_metadata *meta)
+	{
 		//XXX
-		Publisher * pub = new Publisher(meta, &_pubs);
+		Publisher *pub = new Publisher(meta, &_pubs);
 		return pub;
 	}
 
-	void spinOnce() {
+	/**
+	 * Calls all callback waiting to be called
+	 */
+	void spinOnce()
+	{
 		/* Loop through subscriptions, call callback for updated subscriptions */
 		uORB::SubscriptionNode *sub = _subs.getHead();
 		int count = 0;
@@ -152,9 +194,14 @@ public:
 		}
 	}
 
-	void spin() {
+	/**
+	 * Keeps calling callbacks for incomming messages, returns when module is terminated
+	 */
+	void spin()
+	{
 		while (ok()) {
 			const int timeout_ms = 100;
+
 			/* Only continue in the loop if the nodehandle has subscriptions */
 			if (_sub_min_interval == nullptr) {
 				usleep(timeout_ms * 1000);
@@ -165,6 +212,7 @@ public:
 			struct pollfd pfd;
 			pfd.fd = _sub_min_interval->getHandle();
 			pfd.events = POLLIN;
+
 			if (poll(&pfd, 1, timeout_ms) <= 0) {
 				/* timed out */
 				continue;
@@ -175,9 +223,9 @@ public:
 	}
 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
+	List<uORB::SubscriptionNode *> _subs;		/**< Subcriptions of node */
+	List<uORB::PublicationNode *> _pubs;		/**< Publications of node */
+	uORB::SubscriptionNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval
 							  of all Subscriptions in _subs*/
 };
 #endif
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 9ce211d25..cb15eeb58 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -52,33 +52,54 @@ namespace px4
 class Publisher
 {
 public:
+	/**
+	 * Construct Publisher by providing a ros::Publisher
+	 * @param ros_pub the ros publisher which will be used to perform the publications
+	 */
 	Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub)
 	{}
+
 	~Publisher() {};
+
+	/** Publishes msg
+	 * @param msg	    the message which is published to the topic
+	 */
 	template<typename M>
 	int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
 private:
-	ros::Publisher _ros_pub;
+	ros::Publisher _ros_pub;	/**< Handle to the ros publisher */
 };
 #else
 class Publisher :
 	public uORB::PublicationNode
 {
 public:
+	/**
+	 * Construct Publisher by providing orb meta data
+	 * @param meta	    orb metadata for the topic which is used
+	 * @param list	    publisher is added to this list
+	 */
 	Publisher(const struct orb_metadata *meta,
-			List<uORB::PublicationNode *> * list) :
+		  List<uORB::PublicationNode *> *list) :
 		uORB::PublicationNode(meta, list)
 	{}
+
 	~Publisher() {};
+
+	/** Publishes msg
+	 * @param msg	    the message which is published to the topic
+	 */
 	template<typename M>
-	int publish(const M &msg) {
-		uORB::PublicationBase::update((void*)&msg);
+	int publish(const M &msg)
+	{
+		uORB::PublicationBase::update((void *)&msg);
 		return 0;
 	}
 
-	void update() {
-	//XXX list traversal callback, needed?
-	} ;
+	/**
+	 * Empty callback for list traversal
+	 */
+	void update() {} ;
 };
 #endif
 }
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 6e0ef8aed..e995c6e49 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -54,15 +54,24 @@ namespace px4
 class Subscriber
 {
 public:
+	/**
+	 * Construct Subscriber by providing a ros::Subscriber
+	 * @param ros_sub the ros subscriber which will be used to perform the publications
+	 */
 	Subscriber(ros::Subscriber ros_sub) :
 		_ros_sub(ros_sub)
 	{}
+
 	~Subscriber() {};
 private:
-	ros::Subscriber _ros_sub;
+	ros::Subscriber _ros_sub;	/**< Handle to ros subscriber */
 };
+
 #else
-// typedef std::function<void(int)> CallbackFunction;
+
+/**
+ * Subscriber class which is used by nodehandle when building for NuttX
+ */
 class Subscriber
 {
 public:
@@ -71,24 +80,41 @@ public:
 private:
 };
 
+/**
+ * Subscriber class that is templated with the uorb subscription message type
+ */
 template<typename M>
 class SubscriberPX4 :
 	public Subscriber,
 	public uORB::Subscription<M>
 {
 public:
+	/**
+	 * Construct SubscriberPX4 by providing orb meta data
+	 * @param meta	    orb metadata for the topic which is used
+	 * @param callback	Callback, executed on receiving a new message
+	 * @param interval	Minimal interval between calls to callback
+	 * @param list	    subscriber is added to this list
+	 */
 	SubscriberPX4(const struct orb_metadata *meta,
-			unsigned interval,
-			std::function<void(const M&)> callback,
-			List<uORB::SubscriptionNode *> * list) :
+		      unsigned interval,
+		      std::function<void(const M &)> callback,
+		      List<uORB::SubscriptionNode *> *list) :
 		Subscriber(),
 		uORB::Subscription<M>(meta, interval, list),
 		_callback(callback)
 		//XXX store callback
 	{}
+
 	~SubscriberPX4() {};
 
-	void update() {
+	/**
+	 * Update Subscription
+	 * Invoked by the list traversal in NodeHandle::spinOnce
+	 * If new data is available the callback is called
+	 */
+	void update()
+	{
 		if (!uORB::Subscription<M>::updated()) {
 			/* Topic not updated, do not call callback */
 			return;
@@ -102,7 +128,8 @@ public:
 
 	};
 private:
-	std::function<void(const M&)> _callback;
+	std::function<void(const M &)> _callback;	/**< Callback handle,
+							  called when new data is available */
 
 };
 #endif