updated to master (solved merge conflicts)

src/modules/uORB/Publication.cpp

@@ -50,37 +50,63 @@
 #include "topics/encoders.h"
 #include "topics/tecs_status.h"
 #include "topics/rc_channels.h"
+#include "topics/filtered_bottom_flow.h"
+
+#include <px4_defines.h>
 
 namespace uORB
 {
 
-template<class T>
-Publication<T>::Publication(
-	const struct orb_metadata *meta,
-	List<PublicationNode *> *list) :
-	T(), // initialize data structure to zero
-	PublicationNode(meta, list)
+PublicationBase::PublicationBase(const struct orb_metadata *meta,
+				 int priority) :
+	_meta(meta),
+	_priority(priority),
+	_instance(),
+	_handle(nullptr)
 {
 }
 
-template<class T>
-Publication<T>::~Publication() {}
-
-template<class T>
-void *Publication<T>::getDataVoidPtr()
+void PublicationBase::update(void *data)
 {
-	return (void *)(T *)(this);
+	if (_handle != nullptr) {
+		int ret = orb_publish(getMeta(), getHandle(), data);
+
+		if (ret != PX4_OK) { warnx("publish fail"); }
+
+	} else {
+		orb_advert_t handle;
+
+		if (_priority > 0) {
+			handle = orb_advertise_multi(
+					 getMeta(), data,
+					 &_instance, _priority);
+
+		} else {
+			handle = orb_advertise(getMeta(), data);
+		}
+
+		if (int64_t(handle) != PX4_ERROR) {
+			setHandle(handle);
+
+		} else {
+			warnx("advert fail");
+		}
+	}
 }
 
+PublicationBase::~PublicationBase()
+{
+}
 
 PublicationNode::PublicationNode(const struct orb_metadata *meta,
+				 int priority,
 				 List<PublicationNode *> *list) :
-	PublicationBase(meta)
+	PublicationBase(meta, priority)
 {
 	if (list != nullptr) { list->add(this); }
 }
 
-
+// explicit template instantiation
 template class __EXPORT Publication<vehicle_attitude_s>;
 template class __EXPORT Publication<vehicle_local_position_s>;
 template class __EXPORT Publication<vehicle_global_position_s>;
@@ -94,5 +120,6 @@ template class __EXPORT Publication<actuator_direct_s>;
 template class __EXPORT Publication<encoders_s>;
 template class __EXPORT Publication<tecs_status_s>;
 template class __EXPORT Publication<rc_channels_s>;
+template class __EXPORT Publication<filtered_bottom_flow_s>;
 
 }

src/modules/uORB/Publication.hpp

@@ -42,13 +42,13 @@
 
 #include <uORB/uORB.h>
 #include <containers/List.hpp>
-
+#include <systemlib/err.h>
 
 namespace uORB
 {
 
 /**
- * Base publication warapper class, used in list traversal
+ * Base publication wrapper class, used in list traversal
  * of various publications.
  */
 class __EXPORT PublicationBase
@@ -58,50 +58,40 @@ public:
 	/**
 	 * Constructor
 	 *
-	 *
-	 * @param meta The uORB metadata (usually from the ORB_ID()
-	 * 	macro) for the topic.
+	 * @param meta The uORB metadata (usually from
+	 * 	the ORB_ID() macro) for the topic.
+	 * @param priority The priority for multi pub/sub, 0-based, -1 means
+	 * 	don't publish as multi
 	 */
-	PublicationBase(const struct orb_metadata *meta) :
-		_meta(meta),
-		_handle(nullptr)
-	{
-	}
+	PublicationBase(const struct orb_metadata *meta,
+			int priority = -1);
 
 	/**
 	 * Update the struct
 	 * @param data The uORB message struct we are updating.
 	 */
-	void update(void *data)
-	{
-		if (_handle != nullptr) {
-			orb_publish(getMeta(), getHandle(), data);
-
-		} else {
-			setHandle(orb_advertise(getMeta(), data));
-		}
-	}
+	void update(void *data);
 
 	/**
 	 * Deconstructor
 	 */
-	virtual ~PublicationBase()
-	{
-	}
+	virtual ~PublicationBase();
+
 // accessors
 	const struct orb_metadata *getMeta() { return _meta; }
 	orb_advert_t getHandle() { return _handle; }
 protected:
+	// disallow copy
+	PublicationBase(const PublicationBase &other);
+	// disallow assignment
+	PublicationBase &operator=(const PublicationBase &other);
 // accessors
 	void setHandle(orb_advert_t handle) { _handle = handle; }
 // attributes
 	const struct orb_metadata *_meta;
+	int _priority;
+	int _instance;
 	orb_advert_t _handle;
-private:
-	// forbid copy
-	PublicationBase(const PublicationBase &) : _meta(), _handle() {};
-	// forbid assignment
-	PublicationBase &operator = (const PublicationBase &);
 };
 
 /**
@@ -121,13 +111,14 @@ public:
 	/**
 	 * Constructor
 	 *
-	 *
-	 * @param meta The uORB metadata (usually from the ORB_ID()
-	 * 	macro) for the topic.
-	 * @param list 	A pointer to a list of subscriptions
-	 * 	that this should be appended to.
+	 * @param meta The uORB metadata (usually from
+	 * 	the ORB_ID() macro) for the topic.
+	 * @param priority The priority for multi pub, 0-based.
+	 * @param list A list interface for adding to
+	 * 	list during construction
 	 */
 	PublicationNode(const struct orb_metadata *meta,
+			int priority = -1,
 			List<PublicationNode *> *list = nullptr);
 
 	/**
@@ -142,7 +133,6 @@ public:
  */
 template<class T>
 class __EXPORT Publication :
-	public T, // this must be first!
 	public PublicationNode
 {
 public:
@@ -151,33 +141,37 @@ public:
 	 *
 	 * @param meta The uORB metadata (usually from
 	 * 	the ORB_ID() macro) for the topic.
+	 * @param priority The priority for multi pub, 0-based.
 	 * @param list A list interface for adding to
 	 * 	list during construction
 	 */
 	Publication(const struct orb_metadata *meta,
-		    List<PublicationNode *> *list = nullptr);
+		    int priority = -1,
+		    List<PublicationNode *> *list = nullptr)  :
+		PublicationNode(meta, priority, list),
+		_data()
+	{
+	}
 
 	/**
 	 * Deconstructor
 	 **/
-	virtual ~Publication();
+	virtual ~Publication() {};
 
 	/*
-	 * XXX
-	 * This function gets the T struct, assuming
-	 * the struct is the first base class, this
-	 * should use dynamic cast, but doesn't
-	 * seem to be available
-	 */
-	void *getDataVoidPtr();
+	 * This function gets the T struct
+	 * */
+	T &get() { return _data; }
 
 	/**
 	 * Create an update function that uses the embedded struct.
 	 */
 	void update()
 	{
-		PublicationBase::update(getDataVoidPtr());
+		PublicationBase::update((void *)(&_data));
 	}
+private:
+	T _data;
 };
 
 } // namespace uORB

src/modules/uORB/Subscription.cpp

@@ -54,37 +54,89 @@
 #include "topics/vehicle_attitude_setpoint.h"
 #include "topics/vehicle_rates_setpoint.h"
 #include "topics/rc_channels.h"
+#include "topics/battery_status.h"
+#include "topics/optical_flow.h"
+#include "topics/distance_sensor.h"
+#include "topics/home_position.h"
 #include "topics/vehicle_control_mode.h"
 #include "topics/actuator_armed.h"
+#include "topics/att_pos_mocap.h"
+#include "topics/vision_position_estimate.h"
+
+#include <px4_defines.h>
 
 namespace uORB
 {
 
-template<class T>
-Subscription<T>::Subscription(
-	const struct orb_metadata *meta,
-	unsigned interval,
-	List<SubscriptionNode *> *list) :
-	T(), // initialize data structure to zero
-	SubscriptionNode(meta, interval, list)
+SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta,
+				   unsigned interval, unsigned instance) :
+	_meta(meta),
+	_instance(instance),
+	_handle()
+{
+	if (_instance > 0) {
+		_handle =  orb_subscribe_multi(
+				   getMeta(), instance);
+
+	} else {
+		_handle =  orb_subscribe(getMeta());
+	}
+
+	if (_handle < 0) { warnx("sub failed"); }
+
+	orb_set_interval(getHandle(), interval);
+}
+
+bool SubscriptionBase::updated()
+{
+	bool isUpdated = false;
+	int ret = orb_check(_handle, &isUpdated);
+
+	if (ret != PX4_OK) { warnx("orb check failed"); }
+
+	return isUpdated;
+}
+
+void SubscriptionBase::update(void *data)
+{
+	if (updated()) {
+		int ret = orb_copy(_meta, _handle, data);
+
+		if (ret != PX4_OK) { warnx("orb copy failed"); }
+	}
+}
+
+SubscriptionBase::~SubscriptionBase()
+{
+	int ret = orb_unsubscribe(_handle);
+
+	if (ret != PX4_OK) { warnx("orb unsubscribe failed"); }
+}
+
+template <class T>
+Subscription<T>::Subscription(const struct orb_metadata *meta,
+			      unsigned interval,
+			      int instance,
+			      List<SubscriptionNode *> *list) :
+	SubscriptionNode(meta, interval, instance, list),
+	_data() // initialize data structure to zero
 {
 }
 
-template<class T>
-Subscription<T>::~Subscription() {}
-
-template<class T>
-void *Subscription<T>::getDataVoidPtr()
+template <class T>
+Subscription<T>::~Subscription()
 {
-	return (void *)(T *)(this);
 }
 
-template<class T>
-T Subscription<T>::getData()
+template <class T>
+void Subscription<T>::update()
 {
-	return T(*this);
+	SubscriptionBase::update((void *)(&_data));
 }
 
+template <class T>
+const T &Subscription<T>::get() { return _data; }
+
 template class __EXPORT Subscription<parameter_update_s>;
 template class __EXPORT Subscription<actuator_controls_s>;
 template class __EXPORT Subscription<vehicle_gps_position_s>;
@@ -104,5 +156,11 @@ template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
 template class __EXPORT Subscription<rc_channels_s>;
 template class __EXPORT Subscription<vehicle_control_mode_s>;
 template class __EXPORT Subscription<actuator_armed_s>;
+template class __EXPORT Subscription<battery_status_s>;
+template class __EXPORT Subscription<home_position_s>;
+template class __EXPORT Subscription<optical_flow_s>;
+template class __EXPORT Subscription<distance_sensor_s>;
+template class __EXPORT Subscription<att_pos_mocap_s>;
+template class __EXPORT Subscription<vision_position_estimate_s>;
 
 } // namespace uORB

src/modules/uORB/Subscription.hpp

@@ -42,6 +42,7 @@
 
 #include <uORB/uORB.h>
 #include <containers/List.hpp>
+#include <systemlib/err.h>
 
 namespace uORB
 {
@@ -60,43 +61,29 @@ public:
 	 *
 	 * @param meta The uORB metadata (usually from the ORB_ID()
 	 * 	macro) for the topic.
-	 *
 	 * @param interval  The minimum interval in milliseconds
 	 * 	between updates
+	 * @param instance The instance for multi sub.
 	 */
 	SubscriptionBase(const struct orb_metadata *meta,
-		unsigned interval=0) :
-		_meta(meta),
-		_handle() {
-		setHandle(orb_subscribe(getMeta()));
-		orb_set_interval(getHandle(), interval);
-	}
+			 unsigned interval = 0, unsigned instance = 0);
 
 	/**
 	 * Check if there is a new update.
 	 * */
-	bool updated() {
-		bool isUpdated = false;
-		orb_check(_handle, &isUpdated);
-		return isUpdated;
-	}
+	bool updated();
 
 	/**
 	 * Update the struct
 	 * @param data The uORB message struct we are updating.
 	 */
-	void update(void * data) {
-		if (updated()) {
-			orb_copy(_meta, _handle, data);
-		}
-	}
+	void update(void *data);
 
 	/**
 	 * Deconstructor
 	 */
-	virtual ~SubscriptionBase() {
-		orb_unsubscribe(_handle);
-	}
+	virtual ~SubscriptionBase();
+
 // accessors
 	const struct orb_metadata *getMeta() { return _meta; }
 	int getHandle() { return _handle; }
@@ -105,12 +92,13 @@ protected:
 	void setHandle(int handle) { _handle = handle; }
 // attributes
 	const struct orb_metadata *_meta;
+	int _instance;
 	int _handle;
 private:
-	// forbid copy
-	SubscriptionBase(const SubscriptionBase& other);
-	// forbid assignment
-	SubscriptionBase& operator = (const SubscriptionBase &);
+	// disallow copy
+	SubscriptionBase(const SubscriptionBase &other);
+	// disallow assignment
+	SubscriptionBase &operator=(const SubscriptionBase &other);
 };
 
 /**
@@ -119,7 +107,7 @@ private:
 typedef SubscriptionBase SubscriptionTiny;
 
 /**
- * The publication base class as a list node.
+ * The subscription base class as a list node.
  */
 class __EXPORT SubscriptionNode :
 
@@ -130,20 +118,22 @@ public:
 	/**
 	 * Constructor
 	 *
-	 *
 	 * @param meta The uORB metadata (usually from the ORB_ID()
 	 * 	macro) for the topic.
 	 * @param interval  The minimum interval in milliseconds
 	 * 	between updates
+	 * @param instance The instance for multi sub.
 	 * @param list 	A pointer to a list of subscriptions
 	 * 	that this should be appended to.
 	 */
 	SubscriptionNode(const struct orb_metadata *meta,
-		unsigned interval=0,
-		List<SubscriptionNode *> * list=nullptr) :
-		SubscriptionBase(meta, interval),
-		_interval(interval) {
-		if (list != nullptr) list->add(this);
+			 unsigned interval = 0,
+			 int instance = 0,
+			 List<SubscriptionNode *> *list = nullptr) :
+		SubscriptionBase(meta, interval, instance),
+		_interval(interval)
+	{
+		if (list != nullptr) { list->add(this); }
 	}
 
 	/**
@@ -164,7 +154,6 @@ protected:
  */
 template<class T>
 class __EXPORT Subscription :
-	public T, // this must be first!
 	public SubscriptionNode
 {
 public:
@@ -179,8 +168,10 @@ public:
 	 * 	list during construction
 	 */
 	Subscription(const struct orb_metadata *meta,
-		unsigned interval=0,
-		List<SubscriptionNode *> * list=nullptr);
+		     unsigned interval = 0,
+		     int instance = 0,
+		     List<SubscriptionNode *> *list = nullptr);
+
 	/**
 	 * Deconstructor
 	 */
@@ -190,19 +181,14 @@ public:
 	/**
 	 * Create an update function that uses the embedded struct.
 	 */
-	void update() {
-		SubscriptionBase::update(getDataVoidPtr());
-	}
+	void update();
 
 	/*
-	 * XXX
-	 * This function gets the T struct, assuming
-	 * the struct is the first base class, this
-	 * should use dynamic cast, but doesn't
-	 * seem to be available
-	 */
-	void *getDataVoidPtr();
-	T getData();
+	 * This function gets the T struct data
+	 * */
+	const T &get();
+private:
+	T _data;
 };
 
 } // namespace uORB

src/modules/uORB/uORB.cpp

@@ -269,6 +269,7 @@ int  orb_exists(const struct orb_metadata *meta, int instance)
 int  orb_group_count(const struct orb_metadata *meta)
 {
 	unsigned group_count = 0;
+
 	while (!uORB::Manager::get_instance()->orb_exists(meta, group_count++)) {};
 
 	return group_count;

src/modules/uORB/uORBDevices_nuttx.cpp

@@ -36,7 +36,6 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <nuttx/arch.h>
-#include <algorithm>
 #include "uORBDevices_nuttx.hpp"
 #include "uORBUtils.hpp"
 #include "uORBManager.hpp"
@@ -684,4 +683,3 @@ uORB::DeviceNode *uORB::DeviceMaster::GetDeviceNode(const char *nodepath)
 
 	return rc;
 }
-

src/modules/uORB/uORBMain.cpp

@@ -45,83 +45,86 @@ extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); }
 static uORB::DeviceMaster *g_dev = nullptr;
 static void usage()
 {
-  warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'");
+	warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'");
 }
 
 
 int
 uorb_main(int argc, char *argv[])
 {
-  if (argc < 2) {
-    usage();
-    return -EINVAL;
-  }
+	if (argc < 2) {
+		usage();
+		return -EINVAL;
+	}
 
-  /*
-   * Start/load the driver.
-   *
-   * XXX it would be nice to have a wrapper for this...
-   */
-  if (!strcmp(argv[1], "start")) {
+	/*
+	 * Start/load the driver.
+	 *
+	 * XXX it would be nice to have a wrapper for this...
+	 */
+	if (!strcmp(argv[1], "start")) {
 
-    if (g_dev != nullptr) {
-      warnx("already loaded");
-      /* user wanted to start uorb, its already running, no error */
-      return 0;
-    }
+		if (g_dev != nullptr) {
+			warnx("already loaded");
+			/* user wanted to start uorb, its already running, no error */
+			return 0;
+		}
 
-    /* create the driver */
-    g_dev = new uORB::DeviceMaster(uORB::PUBSUB);
+		/* create the driver */
+		g_dev = new uORB::DeviceMaster(uORB::PUBSUB);
 
-    if (g_dev == nullptr) {
-      warnx("driver alloc failed");
-      return -ENOMEM;
-    }
+		if (g_dev == nullptr) {
+			warnx("driver alloc failed");
+			return -ENOMEM;
+		}
 
-    if (OK != g_dev->init()) {
-      warnx("driver init failed");
-      delete g_dev;
-      g_dev = nullptr;
-      return -EIO;
-    }
+		if (OK != g_dev->init()) {
+			warnx("driver init failed");
+			delete g_dev;
+			g_dev = nullptr;
+			return -EIO;
+		}
 
-    return OK;
-  }
+		return OK;
+	}
 
 #ifndef __PX4_QURT
-  /*
-   * Test the driver/device.
-   */
-  if (!strcmp(argv[1], "test"))
-  {
-    uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
-    return t.test();
-  }
 
-  /*
-   * Test the latency.
-   */
-  if (!strcmp(argv[1], "latency_test")) {
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
+		return t.test();
+	}
+
+	/*
+	 * Test the latency.
+	 */
+	if (!strcmp(argv[1], "latency_test")) {
+
+		uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
+
+		if (argc > 2 && !strcmp(argv[2], "medium")) {
+			return t.latency_test<struct orb_test_medium>(ORB_ID(orb_test_medium), true);
+
+		} else if (argc > 2 && !strcmp(argv[2], "large")) {
+			return t.latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true);
+
+		} else {
+			return t.latency_test<struct orb_test>(ORB_ID(orb_test), true);
+		}
+	}
 
-    uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
-    if (argc > 2 && !strcmp(argv[2], "medium")) {
-      return t.latency_test<struct orb_test_medium>(ORB_ID(orb_test_medium), true);
-    } else if (argc > 2 && !strcmp(argv[2], "large")) {
-      return t.latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true);
-    } else {
-      return t.latency_test<struct orb_test>(ORB_ID(orb_test), true);
-    }
-  }
 #endif
 
-  /*
-   * Print driver information.
-   */
-  if (!strcmp(argv[1], "status"))
-  {
-    return OK;
-  }
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "status")) {
+		return OK;
+	}
 
-  usage();
-  return -EINVAL;
+	usage();
+	return -EINVAL;
 }

src/modules/uORB/uORBUtils.cpp

@@ -37,45 +37,46 @@
 
 int uORB::Utils::node_mkpath
 (
-        char *buf,
-        Flavor f,
-        const struct orb_metadata *meta,
-        int *instance
+	char *buf,
+	Flavor f,
+	const struct orb_metadata *meta,
+	int *instance
 )
 {
-  unsigned len;
+	unsigned len;
 
-  unsigned index = 0;
+	unsigned index = 0;
 
-  if (instance != nullptr) {
-    index = *instance;
-  }
+	if (instance != nullptr) {
+		index = *instance;
+	}
 
-  len = snprintf(buf, orb_maxpath, "/%s/%s%d",
-      (f == PUBSUB) ? "obj" : "param",
-      meta->o_name, index);
+	len = snprintf(buf, orb_maxpath, "/%s/%s%d",
+		       (f == PUBSUB) ? "obj" : "param",
+		       meta->o_name, index);
 
-  if (len >= orb_maxpath) {
-    return -ENAMETOOLONG;
-  }
+	if (len >= orb_maxpath) {
+		return -ENAMETOOLONG;
+	}
 
-  return OK;
+	return OK;
 }
 
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 int uORB::Utils::node_mkpath(char *buf, Flavor f,
-                               const char *orbMsgName )
+			     const char *orbMsgName)
 {
-  unsigned len;
+	unsigned len;
 
-  unsigned index = 0;
+	unsigned index = 0;
 
-  len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param",
-                 orbMsgName, index );
+	len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param",
+		       orbMsgName, index);
 
-  if (len >= orb_maxpath)
-    return -ENAMETOOLONG;
+	if (len >= orb_maxpath) {
+		return -ENAMETOOLONG;
+	}
 
-  return OK;
+	return OK;
 }

src/modules/uORB/uORBUtils.hpp

@@ -38,24 +38,24 @@
 
 namespace uORB
 {
-  class Utils;
+class Utils;
 }
 
 class uORB::Utils
 {
 public:
-  static int node_mkpath
-  (
-      char *buf,
-      Flavor f,
-      const struct orb_metadata *meta,
-      int *instance = nullptr
-  );
+	static int node_mkpath
+	(
+		char *buf,
+		Flavor f,
+		const struct orb_metadata *meta,
+		int *instance = nullptr
+	);
 
-  /**
-   * same as above except this generators the path based on the string.
-   */
-  static int node_mkpath(char *buf, Flavor f, const char *orbMsgName);
+	/**
+	 * same as above except this generators the path based on the string.
+	 */
+	static int node_mkpath(char *buf, Flavor f, const char *orbMsgName);
 
 };