Control lib update.

src/drivers/roboclaw/RoboClaw.cpp

@@ -299,8 +299,8 @@ int RoboClaw::update()
 	// if new data, send to motors
 	if (_actuators.updated()) {
 		_actuators.update();
-		setMotorDutyCycle(MOTOR_1, _actuators.control[CH_VOLTAGE_LEFT]);
-		setMotorDutyCycle(MOTOR_2, _actuators.control[CH_VOLTAGE_RIGHT]);
+		setMotorDutyCycle(MOTOR_1, _actuators.get().control[CH_VOLTAGE_LEFT]);
+		setMotorDutyCycle(MOTOR_2, _actuators.get().control[CH_VOLTAGE_RIGHT]);
 	}
 
 	return 0;

src/modules/controllib/block/Block.hpp

@@ -58,8 +58,8 @@ static const uint16_t maxPublicationsPerBlock = 100;
 static const uint8_t blockNameLengthMax = 80;
 
 // forward declaration
-class SuperBlock;
 class BlockParamBase;
+class SuperBlock;
 
 /**
  */
@@ -81,9 +81,9 @@ public:
 protected:
 // accessors
 	SuperBlock *getParent() { return _parent; }
-	List<uORB::SubscriptionNode *> & getSubscriptions() { return _subscriptions; }
-	List<uORB::PublicationNode *> & getPublications() { return _publications; }
-	List<BlockParamBase *> & getParams() { return _params; }
+	List<uORB::SubscriptionNode *> &getSubscriptions() { return _subscriptions; }
+	List<uORB::PublicationNode *> &getPublications() { return _publications; }
+	List<BlockParamBase *> &getParams() { return _params; }
 // attributes
 	const char *_name;
 	SuperBlock *_parent;
@@ -94,8 +94,8 @@ protected:
 
 private:
 	/* this class has pointer data members and should not be copied (private constructor) */
-	Block(const control::Block&);
-	Block operator=(const control::Block&);
+	Block(const control::Block &);
+	Block operator=(const control::Block &);
 };
 
 class __EXPORT SuperBlock :
@@ -106,28 +106,32 @@ public:
 // methods
 	SuperBlock(SuperBlock *parent, const char *name) :
 		Block(parent, name),
-		_children() {
+		_children()
+	{
 	}
 	virtual ~SuperBlock() {};
 	virtual void setDt(float dt);
-	virtual void updateParams() {
+	virtual void updateParams()
+	{
 		Block::updateParams();
 
-		if (getChildren().getHead() != NULL) updateChildParams();
+		if (getChildren().getHead() != NULL) { updateChildParams(); }
 	}
-	virtual void updateSubscriptions() {
+	virtual void updateSubscriptions()
+	{
 		Block::updateSubscriptions();
 
-		if (getChildren().getHead() != NULL) updateChildSubscriptions();
+		if (getChildren().getHead() != NULL) { updateChildSubscriptions(); }
 	}
-	virtual void updatePublications() {
+	virtual void updatePublications()
+	{
 		Block::updatePublications();
 
-		if (getChildren().getHead() != NULL) updateChildPublications();
+		if (getChildren().getHead() != NULL) { updateChildPublications(); }
 	}
 protected:
 // methods
-	List<Block *> & getChildren() { return _children; }
+	List<Block *> &getChildren() { return _children; }
 	void updateChildParams();
 	void updateChildSubscriptions();
 	void updateChildPublications();

src/modules/controllib/block/BlockParam.cpp

@@ -65,6 +65,7 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
 
 		} else if (parent_prefix) {
 			snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
+
 		} else {
 			strncpy(fullname, name, blockNameLengthMax);
 		}
@@ -74,15 +75,17 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
 
 	_handle = param_find(fullname);
 
-	if (_handle == PARAM_INVALID)
+	if (_handle == PARAM_INVALID) {
 		printf("error finding param: %s\n", fullname);
+	}
 };
 
 template <class T>
 BlockParam<T>::BlockParam(Block *block, const char *name,
-		bool parent_prefix) :
+			  bool parent_prefix) :
 	BlockParamBase(block, name, parent_prefix),
-	_val() {
+	_val()
+{
 	update();
 }
 
@@ -93,13 +96,15 @@ template <class T>
 void BlockParam<T>::set(T val) { _val = val; }
 
 template <class T>
-void BlockParam<T>::update() {
-	if (_handle != PARAM_INVALID) param_get(_handle, &_val);
+void BlockParam<T>::update()
+{
+	if (_handle != PARAM_INVALID) { param_get(_handle, &_val); }
 }
 
 template <class T>
-void BlockParam<T>::commit() {
-	if (_handle != PARAM_INVALID) param_set(_handle, &_val);
+void BlockParam<T>::commit()
+{
+	if (_handle != PARAM_INVALID) { param_set(_handle, &_val); }
 }
 
 template <class T>

src/modules/controllib/blocks.cpp

@@ -126,6 +126,7 @@ float BlockLowPass::update(float input)
 	if (!isfinite(getState())) {
 		setState(input);
 	}
+
 	float b = 2 * float(M_PI) * getFCut() * getDt();
 	float a = b / (1 + b);
 	setState(a * input + (1 - a)*getState());
@@ -209,6 +210,7 @@ float BlockLowPass2::update(float input)
 	if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
 		_lp.set_cutoff_frequency(_fs, getFCutParam());
 	}
+
 	_state = _lp.apply(input);
 	return _state;
 }
@@ -340,8 +342,10 @@ int blockIntegralTrapTest()
 float BlockDerivative::update(float input)
 {
 	float output;
+
 	if (_initialized) {
 		output = _lowPass.update((input - getU()) / getDt());
+
 	} else {
 		// if this is the first call to update
 		// we have no valid derivative
@@ -351,6 +355,7 @@ float BlockDerivative::update(float input)
 		output = 0.0f;
 		_initialized = true;
 	}
+
 	setU(input);
 	return output;
 }

src/modules/controllib/blocks.hpp

@@ -317,7 +317,8 @@ public:
 		_kP(this, "") // only one param, no need to name
 	{};
 	virtual ~BlockP() {};
-	float update(float input) {
+	float update(float input)
+	{
 		return getKP() * input;
 	}
 // accessors
@@ -343,7 +344,8 @@ public:
 		_kI(this, "I")
 	{};
 	virtual ~BlockPI() {};
-	float update(float input) {
+	float update(float input)
+	{
 		return getKP() * input +
 		       getKI() * getIntegral().update(input);
 	}
@@ -374,7 +376,8 @@ public:
 		_kD(this, "D")
 	{};
 	virtual ~BlockPD() {};
-	float update(float input) {
+	float update(float input)
+	{
 		return getKP() * input +
 		       getKD() * getDerivative().update(input);
 	}
@@ -407,7 +410,8 @@ public:
 		_kD(this, "D")
 	{};
 	virtual ~BlockPID() {};
-	float update(float input) {
+	float update(float input)
+	{
 		return getKP() * input +
 		       getKI() * getIntegral().update(input) +
 		       getKD() * getDerivative().update(input);
@@ -440,11 +444,13 @@ public:
 		SuperBlock(parent, name),
 		_trim(this, "TRIM"),
 		_limit(this, ""),
-		_val(0) {
+		_val(0)
+	{
 		update(0);
 	};
 	virtual ~BlockOutput() {};
-	void update(float input) {
+	void update(float input)
+	{
 		_val = _limit.update(input + getTrim());
 	}
 // accessors
@@ -472,13 +478,15 @@ public:
 			 const char *name) :
 		Block(parent, name),
 		_min(this, "MIN"),
-		_max(this, "MAX") {
+		_max(this, "MAX")
+	{
 		// seed should be initialized somewhere
 		// in main program for all calls to rand
 		// XXX currently in nuttx if you seed to 0, rand breaks
 	};
 	virtual ~BlockRandUniform() {};
-	float update() {
+	float update()
+	{
 		static float rand_max = MAX_RAND;
 		float rand_val = rand();
 		float bounds = getMax() - getMin();
@@ -503,13 +511,15 @@ public:
 		       const char *name) :
 		Block(parent, name),
 		_mean(this, "MEAN"),
-		_stdDev(this, "DEV") {
+		_stdDev(this, "DEV")
+	{
 		// seed should be initialized somewhere
 		// in main program for all calls to rand
 		// XXX currently in nuttx if you seed to 0, rand breaks
 	};
 	virtual ~BlockRandGauss() {};
-	float update() {
+	float update()
+	{
 		static float V1, V2, S;
 		static int phase = 0;
 		float X;

src/modules/controllib/uorb/blocks.cpp

@@ -53,10 +53,11 @@ BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *nam
 
 BlockWaypointGuidance::~BlockWaypointGuidance() {};
 
-void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
-				   vehicle_attitude_s &att,
-				   position_setpoint_s &missionCmd,
-				   position_setpoint_s &lastMissionCmd)
+void BlockWaypointGuidance::update(
+	const vehicle_global_position_s &pos,
+	const vehicle_attitude_s &att,
+	const position_setpoint_s &missionCmd,
+	const position_setpoint_s &lastMissionCmd)
 {
 
 	// heading to waypoint
@@ -83,16 +84,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
 BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
 	// subscriptions
-	_att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()),
-	_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()),
-	_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()),
-	_pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()),
-	_missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()),
-	_manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()),
-	_status(ORB_ID(vehicle_status), 20, &getSubscriptions()),
-	_param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz
+	_att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()),
+	_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()),
+	_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()),
+	_pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()),
+	_missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()),
+	_manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()),
+	_status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()),
+	_param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz
 	// publications
-	_actuators(ORB_ID(actuator_controls_0), &getPublications())
+	_actuators(ORB_ID(actuator_controls_0), -1, &getPublications())
 {
 }
 

src/modules/controllib/uorb/blocks.hpp

@@ -80,10 +80,10 @@ private:
 public:
 	BlockWaypointGuidance(SuperBlock *parent, const char *name);
 	virtual ~BlockWaypointGuidance();
-	void update(vehicle_global_position_s &pos,
-		    vehicle_attitude_s &att,
-		    position_setpoint_s &missionCmd,
-		    position_setpoint_s &lastMissionCmd);
+	void update(const vehicle_global_position_s &pos,
+		    const vehicle_attitude_s &att,
+		    const position_setpoint_s &missionCmd,
+		    const position_setpoint_s &lastMissionCmd);
 	float getPsiCmd() { return _psiCmd; }
 };
 

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -53,7 +53,7 @@ mTecs::mTecs() :
 	_airspeedMin(this, "FW_AIRSPD_MIN", false),
 	/* Publications */
 #if defined(__PX4_NUTTX)
-	_status(ORB_ID(tecs_status), &getPublications()),
+	_status(ORB_ID(tecs_status), -1, &getPublications()),
 #endif // defined(__PX4_NUTTX)
 	/* control blocks */
 	_controlTotalEnergy(this, "THR"),
@@ -111,8 +111,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 
 #if defined(__PX4_NUTTX)
 	/* Write part of the status message */
-	_status.altitudeSp = altitudeSp;
-	_status.altitude_filtered = altitudeFiltered;
+	_status.get().altitudeSp = altitudeSp;
+	_status.get().altitude_filtered = altitudeFiltered;
 #endif // defined(__PX4_NUTTX)
 
 
@@ -149,8 +149,8 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 
 #if defined(__PX4_NUTTX)
 	/* Write part of the status message */
-	_status.airspeedSp = airspeedSp;
-	_status.airspeed_filtered = airspeedFiltered;
+	_status.get().airspeedSp = airspeedSp;
+	_status.get().airspeed_filtered = airspeedFiltered;
 #endif // defined(__PX4_NUTTX)
 
 

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,47 +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; }
@@ -109,12 +92,13 @@ protected:
 	void setHandle(int handle) { _handle = handle; }
 // attributes
 	const struct orb_metadata *_meta;
+	int _instance;
 	int _handle;
 private:
-	// forbid copy
+	// disallow copy
 	SubscriptionBase(const SubscriptionBase &other);
-	// forbid assignment
-	SubscriptionBase &operator = (const SubscriptionBase &);
+	// disallow assignment
+	SubscriptionBase &operator=(const SubscriptionBase &other);
 };
 
 /**
@@ -123,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 :
 
@@ -134,18 +118,19 @@ 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,
+			 int instance = 0,
 			 List<SubscriptionNode *> *list = nullptr) :
-		SubscriptionBase(meta, interval),
+		SubscriptionBase(meta, interval, instance),
 		_interval(interval)
 	{
 		if (list != nullptr) { list->add(this); }
@@ -169,7 +154,6 @@ protected:
  */
 template<class T>
 class __EXPORT Subscription :
-	public T, // this must be first!
 	public SubscriptionNode
 {
 public:
@@ -185,7 +169,9 @@ public:
 	 */
 	Subscription(const struct orb_metadata *meta,
 		     unsigned interval = 0,
+		     int instance = 0,
 		     List<SubscriptionNode *> *list = nullptr);
+
 	/**
 	 * Deconstructor
 	 */
@@ -195,20 +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