diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index a3d2f96a20..f9116f1d45 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/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;
diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp
index bb2a0e2657..e74258b523 100644
--- a/src/modules/controllib/block/Block.hpp
+++ b/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();
diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp
index 80f1bae1b0..b560d7999e 100644
--- a/src/modules/controllib/block/BlockParam.cpp
+++ b/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>
diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index 6a0b615910..1aa60e63c8 100644
--- a/src/modules/controllib/blocks.cpp
+++ b/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;
 }
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 786bfc06d3..2b571de590 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/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;
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
index 4f651777ba..3a99617e4d 100644
--- a/src/modules/controllib/uorb/blocks.cpp
+++ b/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())
 {
 }
 
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index e3c0811116..7766b67f6b 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/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; }
 };
 
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index 7984bb12af..39b624f24a 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -142,7 +142,7 @@ void BlockMultiModeBacksideAutopilot::update()
 
 	// store old position command before update if new command sent
 	if (_missionCmd.updated()) {
-		_lastMissionCmd = _missionCmd.getData();
+		_lastMissionCmd = _missionCmd.get();
 	}
 
 	// check for new updates
@@ -152,15 +152,17 @@ void BlockMultiModeBacksideAutopilot::update()
 	updateSubscriptions();
 
 	// default all output to zero unless handled by mode
-	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) {
-		_actuators.control[i] = 0.0f;
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
+		_actuators.get().control[i] = 0.0f;
 	}
 
 	// only update guidance in auto mode
-	if (_status.main_state == MAIN_STATE_AUTO) {
+	if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) {
 		// TODO use vehicle_control_mode here?
 		// update guidance
-		_guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current);
+		_guide.update(_pos.get(), _att.get(),
+			      _missionCmd.get().current,
+			      _lastMissionCmd.current);
 	}
 
 	// XXX handle STABILIZED (loiter on spot) as well
@@ -168,32 +170,32 @@ void BlockMultiModeBacksideAutopilot::update()
 	// the setpoint should update to loitering around this position
 
 	// handle autopilot modes
-	if (_status.main_state == MAIN_STATE_AUTO) {
+	if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO) {
 
 		// calculate velocity, XXX should be airspeed,
 		// but using ground speed for now for the purpose
 		// of control we will limit the velocity feedback between
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
-						 _pos.vel_n * _pos.vel_n +
-						 _pos.vel_e * _pos.vel_e +
-						 _pos.vel_d * _pos.vel_d));
+						 _pos.get().vel_n * _pos.get().vel_n +
+						 _pos.get().vel_e * _pos.get().vel_e +
+						 _pos.get().vel_d * _pos.get().vel_d));
 
 		// limit velocity command between min/max velocity
 		float vCmd = _vLimit.update(_vCmd.get());
 
 		// altitude hold
-		float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt);
+		float dThrottle = _h2Thr.update(_missionCmd.get().current.alt - _pos.get().alt);
 
 		// heading hold
-		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
+		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.get().yaw);
 		float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
-		float pCmd = _phi2P.update(phiCmd - _att.roll);
+		float pCmd = _phi2P.update(phiCmd - _att.get().roll);
 
 		// velocity hold
 		// negative sign because nose over to increase speed
 		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
-		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+		float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch);
 
 		// yaw rate cmd
 		float rCmd = 0;
@@ -203,14 +205,16 @@ void BlockMultiModeBacksideAutopilot::update()
 		float outputScale = velocityRatio * velocityRatio;
 		// this term scales the output based on the dynamic pressure change from trim
 		_stabilization.update(pCmd, qCmd, rCmd,
-				      _att.rollspeed, _att.pitchspeed, _att.yawspeed,
+				      _att.get().rollspeed,
+				      _att.get().pitchspeed,
+				      _att.get().yawspeed,
 				      outputScale);
 
 		// output
-		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
-		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
-		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
-		_actuators.control[CH_THR] = dThrottle + _trimThr.get();
+		_actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+		_actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+		_actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+		_actuators.get().control[CH_THR] = dThrottle + _trimThr.get();
 
 		// XXX limit throttle to manual setting (safety) for now.
 		// If it turns out to be confusing, it can be removed later once
@@ -218,28 +222,29 @@ void BlockMultiModeBacksideAutopilot::update()
 		// This is not a hack, but a design choice.
 
 		// do not limit in HIL
-		if (_status.hil_state != HIL_STATE_ON) {
+		if (_status.get().hil_state != _vehicle_status::HIL_STATE_ON) {
 			/* limit to value of manual throttle */
-			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-						     _actuators.control[CH_THR] : _manual.throttle;
+			_actuators.get().control[CH_THR] =
+				(_actuators.get().control[CH_THR] < _manual.get().z) ?
+				_actuators.get().control[CH_THR] : _manual.get().z;
 		}
 
-	} else if (_status.main_state == MAIN_STATE_MANUAL) {
-		_actuators.control[CH_AIL] = _manual.roll;
-		_actuators.control[CH_ELV] = _manual.pitch;
-		_actuators.control[CH_RDR] = _manual.yaw;
-		_actuators.control[CH_THR] = _manual.throttle;
+	} else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_MANUAL) {
+		_actuators.get().control[CH_AIL] = _manual.get().y;
+		_actuators.get().control[CH_ELV] = _manual.get().x;
+		_actuators.get().control[CH_RDR] = _manual.get().r;
+		_actuators.get().control[CH_THR] = _manual.get().z;
 
-	} else if (_status.main_state == MAIN_STATE_ALTCTL ||
-		   _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) {
+	} else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_ALTCTL ||
+		   _status.get().main_state == vehicle_status_s::MAIN_STATE_POSCTL /* TODO, implement pos control */) {
 
 		// calculate velocity, XXX should be airspeed, but using ground speed for now
 		// for the purpose of control we will limit the velocity feedback between
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
-						 _pos.vel_n * _pos.vel_n +
-						 _pos.vel_e * _pos.vel_e +
-						 _pos.vel_d * _pos.vel_d));
+						 _pos.get().vel_n * _pos.get().vel_n +
+						 _pos.get().vel_e * _pos.get().vel_e +
+						 _pos.get().vel_d * _pos.get().vel_d));
 
 		// pitch channel -> rate of climb
 		// TODO, might want to put a gain on this, otherwise commanding
@@ -248,33 +253,35 @@ void BlockMultiModeBacksideAutopilot::update()
 		//_crMax.get()*_manual.pitch - _pos.vz);
 
 		// roll channel -> bank angle
-		float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
-		float pCmd = _phi2P.update(phiCmd - _att.roll);
+		float phiCmd = _phiLimit.update(_manual.get().y * _phiLimit.getMax());
+		float pCmd = _phi2P.update(phiCmd - _att.get().roll);
 
 		// throttle channel -> velocity
 		// negative sign because nose over to increase speed
-		float vCmd = _vLimit.update(_manual.throttle *
+		float vCmd = _vLimit.update(_manual.get().z *
 					    (_vLimit.getMax() - _vLimit.getMin()) +
 					    _vLimit.getMin());
 		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
-		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+		float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch);
 
 		// yaw rate cmd
 		float rCmd = 0;
 
 		// stabilization
 		_stabilization.update(pCmd, qCmd, rCmd,
-				      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+				      _att.get().rollspeed,
+				      _att.get().pitchspeed,
+				      _att.get().yawspeed);
 
 		// output
-		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
-		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
-		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+		_actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+		_actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+		_actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
 
 		// currently using manual throttle
 		// XXX if you enable this watch out, vz might be very noisy
 		//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
-		_actuators.control[CH_THR] = _manual.throttle;
+		_actuators.get().control[CH_THR] = _manual.get().z;
 
 		// XXX limit throttle to manual setting (safety) for now.
 		// If it turns out to be confusing, it can be removed later once
@@ -282,10 +289,11 @@ void BlockMultiModeBacksideAutopilot::update()
 		// This is not a hack, but a design choice.
 
 		/* do not limit in HIL */
-		if (_status.hil_state != HIL_STATE_ON) {
+		if (_status.get().hil_state != vehicle_status_s::HIL_STATE_ON) {
 			/* limit to value of manual throttle */
-			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-						     _actuators.control[CH_THR] : _manual.throttle;
+			_actuators.get().control[CH_THR] =
+				(_actuators.get().control[CH_THR] < _manual.get().z) ?
+				_actuators.get().control[CH_THR] : _manual.get().z;
 		}
 
 		// body rates controller, disabled for now
@@ -294,13 +302,13 @@ void BlockMultiModeBacksideAutopilot::update()
 	} else if (
 		0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {	// TODO use vehicle_control_mode here?
 
-		_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
-				      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+		_stabilization.update(_manual.get().y, _manual.get().x, _manual.get().r,
+				      _att.get().rollspeed, _att.get().pitchspeed, _att.get().yawspeed);
 
-		_actuators.control[CH_AIL] = _stabilization.getAileron();
-		_actuators.control[CH_ELV] = _stabilization.getElevator();
-		_actuators.control[CH_RDR] = _stabilization.getRudder();
-		_actuators.control[CH_THR] = _manual.throttle;
+		_actuators.get().control[CH_AIL] = _stabilization.getAileron();
+		_actuators.get().control[CH_ELV] = _stabilization.getElevator();
+		_actuators.get().control[CH_RDR] = _stabilization.getRudder();
+		_actuators.get().control[CH_THR] = _manual.get().z;
 	}
 
 	// update all publications
@@ -311,8 +319,8 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
 {
 	// send one last publication when destroyed, setting
 	// all output to zero
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
-		_actuators.control[i] = 0.0f;
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
+		_actuators.get().control[i] = 0.0f;
 	}
 
 	updatePublications();
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 49e1d735e0..6de6d9d8f0 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/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)
 
 
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index bd8ecf13b4..e979e3b4f4 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/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>;
 
 }
diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp
index f8af00e96d..37658721ca 100644
--- a/src/modules/uORB/Publication.hpp
+++ b/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
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index 3554b497d7..a85e8da02d 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/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
diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp
index 49fc9918a1..0136cfcacc 100644
--- a/src/modules/uORB/Subscription.hpp
+++ b/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