Merge remote-tracking branch 'upstream/master' into diff_press_filter

Conflicts:
	src/drivers/meas_airspeed/meas_airspeed.cpp

19 files changed, 321 insertions, 230 deletions

diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index 46ee4b6c8..caf93bc78 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -47,8 +47,8 @@
 #include <mathlib/mathlib.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
-#include <controllib/uorb/UOrbSubscription.hpp>
-#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
 
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -138,13 +138,13 @@ protected:
 	math::Matrix<3,3> C_nb;             /**< direction cosine matrix from body to nav frame */
 	math::Quaternion q;         /**< quaternion from body to nav frame */
 	// subscriptions
-	control::UOrbSubscription<sensor_combined_s> _sensors;          /**< sensors sub. */
-	control::UOrbSubscription<vehicle_gps_position_s> _gps;         /**< gps sub. */
-	control::UOrbSubscription<parameter_update_s> _param_update;    /**< parameter update sub. */
+	uORB::Subscription<sensor_combined_s> _sensors;          /**< sensors sub. */
+	uORB::Subscription<vehicle_gps_position_s> _gps;         /**< gps sub. */
+	uORB::Subscription<parameter_update_s> _param_update;    /**< parameter update sub. */
 	// publications
-	control::UOrbPublication<vehicle_global_position_s> _pos;       /**< position pub. */
-	control::UOrbPublication<vehicle_local_position_s> _localPos;   /**< local position pub. */
-	control::UOrbPublication<vehicle_attitude_s> _att;              /**< attitude pub. */
+	uORB::Publication<vehicle_global_position_s> _pos;       /**< position pub. */
+	uORB::Publication<vehicle_local_position_s> _localPos;   /**< local position pub. */
+	uORB::Publication<vehicle_attitude_s> _att;              /**< attitude pub. */
 	// time stamps
 	uint64_t _pubTimeStamp;     /**< output data publication time stamp */
 	uint64_t _predictTimeStamp; /**< prediction time stamp */
diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp
index b964d40a3..6768bfa7e 100644
--- a/src/modules/controllib/block/Block.cpp
+++ b/src/modules/controllib/block/Block.cpp
@@ -41,10 +41,11 @@
 #include <string.h>
 #include <stdio.h>
 
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+
 #include "Block.hpp"
 #include "BlockParam.hpp"
-#include "../uorb/UOrbSubscription.hpp"
-#include "../uorb/UOrbPublication.hpp"
 
 namespace control
 {
@@ -100,7 +101,7 @@ void Block::updateParams()
 
 void Block::updateSubscriptions()
 {
-	UOrbSubscriptionBase *sub = getSubscriptions().getHead();
+	uORB::SubscriptionBase *sub = getSubscriptions().getHead();
 	int count = 0;
 
 	while (sub != NULL) {
@@ -118,7 +119,7 @@ void Block::updateSubscriptions()
 
 void Block::updatePublications()
 {
-	UOrbPublicationBase *pub = getPublications().getHead();
+	uORB::PublicationBase *pub = getPublications().getHead();
 	int count = 0;
 
 	while (pub != NULL) {
diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp
index 258701f27..736698e21 100644
--- a/src/modules/controllib/block/Block.hpp
+++ b/src/modules/controllib/block/Block.hpp
@@ -42,7 +42,13 @@
 #include <stdint.h>
 #include <inttypes.h>
 
-#include "List.hpp"
+#include <containers/List.hpp>
+
+// forward declaration
+namespace uORB {
+	class SubscriptionBase;
+	class PublicationBase;
+}
 
 namespace control
 {
@@ -55,8 +61,6 @@ static const uint8_t blockNameLengthMax = 80;
 
 // forward declaration
 class BlockParamBase;
-class UOrbSubscriptionBase;
-class UOrbPublicationBase;
 class SuperBlock;
 
 /**
@@ -79,15 +83,15 @@ public:
 protected:
 // accessors
 	SuperBlock *getParent() { return _parent; }
-	List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
-	List<UOrbPublicationBase *> & getPublications() { return _publications; }
+	List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; }
+	List<uORB::PublicationBase *> & getPublications() { return _publications; }
 	List<BlockParamBase *> & getParams() { return _params; }
 // attributes
 	const char *_name;
 	SuperBlock *_parent;
 	float _dt;
-	List<UOrbSubscriptionBase *> _subscriptions;
-	List<UOrbPublicationBase *> _publications;
+	List<uORB::SubscriptionBase *> _subscriptions;
+	List<uORB::PublicationBase *> _publications;
 	List<BlockParamBase *> _params;
 };
 
diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp
index fd12e365d..8f98da74f 100644
--- a/src/modules/controllib/block/BlockParam.cpp
+++ b/src/modules/controllib/block/BlockParam.cpp
@@ -76,4 +76,29 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
 		printf("error finding param: %s\n", fullname);
 };
 
+template <class T>
+BlockParam<T>::BlockParam(Block *block, const char *name,
+		bool parent_prefix) :
+	BlockParamBase(block, name, parent_prefix),
+	_val() {
+	update();
+}
+
+template <class T>
+T BlockParam<T>::get() { return _val; }
+
+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);
+}
+
+template <class T>
+BlockParam<T>::~BlockParam() {};
+
+template class __EXPORT BlockParam<float>;
+template class __EXPORT BlockParam<int>;
+
 } // namespace control
diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp
index 36bc8c24b..a64d0139e 100644
--- a/src/modules/controllib/block/BlockParam.hpp
+++ b/src/modules/controllib/block/BlockParam.hpp
@@ -42,7 +42,7 @@
 #include <systemlib/param/param.h>
 
 #include "Block.hpp"
-#include "List.hpp"
+#include <containers/List.hpp>
 
 namespace control
 {
@@ -70,38 +70,21 @@ protected:
  * Parameters that are tied to blocks for updating and nameing.
  */
 
-class __EXPORT BlockParamFloat : public BlockParamBase
+template <class T>
+class BlockParam : public BlockParamBase
 {
 public:
-	BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) :
-		BlockParamBase(block, name, parent_prefix),
-		_val() {
-		update();
-	}
-	float get() { return _val; }
-	void set(float val) { _val = val; }
-	void update() {
-		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
-	}
+	BlockParam(Block *block, const char *name,
+			bool parent_prefix=true);
+	T get();
+	void set(T val);
+	void update();
+	virtual ~BlockParam();
 protected:
-	float _val;
+	T _val;
 };
 
-class __EXPORT BlockParamInt : public BlockParamBase
-{
-public:
-	BlockParamInt(Block *block, const char *name, bool parent_prefix=true) :
-		BlockParamBase(block, name, parent_prefix),
-		_val() {
-		update();
-	}
-	int get() { return _val; }
-	void set(int val) { _val = val; }
-	void update() {
-		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
-	}
-protected:
-	int _val;
-};
+typedef BlockParam<float> BlockParamFloat;
+typedef BlockParam<int> BlockParamInt;
 
 } // namespace control
diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk
index d815a8feb..f0139a4b7 100644
--- a/src/modules/controllib/module.mk
+++ b/src/modules/controllib/module.mk
@@ -37,7 +37,5 @@
 SRCS		 = test_params.c \
 		   block/Block.cpp \
 		   block/BlockParam.cpp \
-		   uorb/UOrbPublication.cpp \
-		   uorb/UOrbSubscription.cpp \
 		   uorb/blocks.cpp \
 		   blocks.cpp
diff --git a/src/modules/controllib/uorb/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp
deleted file mode 100644
index f69b39d90..000000000
--- a/src/modules/controllib/uorb/UOrbPublication.cpp
+++ /dev/null
@@ -1,39 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 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 UOrbPublication.cpp
- *
- */
-
-#include "UOrbPublication.hpp"
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index 7c80c4b2b..a8a70507e 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -62,8 +62,8 @@ extern "C" {
 }
 
 #include "../blocks.hpp"
-#include "UOrbSubscription.hpp"
-#include "UOrbPublication.hpp"
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
 
 namespace control
 {
@@ -94,16 +94,16 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
 {
 protected:
 	// subscriptions
-	UOrbSubscription<vehicle_attitude_s> _att;
-	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
-	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
-	UOrbSubscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<position_setpoint_triplet_s> _missionCmd;
-	UOrbSubscription<manual_control_setpoint_s> _manual;
-	UOrbSubscription<vehicle_status_s> _status;
-	UOrbSubscription<parameter_update_s> _param_update;
+	uORB::Subscription<vehicle_attitude_s> _att;
+	uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd;
+	uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
+	uORB::Subscription<vehicle_global_position_s> _pos;
+	uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
+	uORB::Subscription<manual_control_setpoint_s> _manual;
+	uORB::Subscription<vehicle_status_s> _status;
+	uORB::Subscription<parameter_update_s> _param_update;
 	// publications
-	UOrbPublication<actuator_controls_s> _actuators;
+	uORB::Publication<actuator_controls_s> _actuators;
 public:
 	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
 	virtual ~BlockUorbEnabledAutopilot();
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index f7c0b6148..cfae07275 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -175,14 +175,14 @@ void BlockMultiModeBacksideAutopilot::update()
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
 					_pos.vel_n * _pos.vel_n +
-					_pos.vy * _pos.vy +
+					_pos.vel_e * _pos.vel_e +
 					_pos.vel_d * _pos.vel_d));
 
 		// limit velocity command between min/max velocity
 		float vCmd = _vLimit.update(_vCmd.get());
 
 		// altitude hold
-		float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt);
+		float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt);
 
 		// heading hold
 		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
@@ -237,7 +237,7 @@ void BlockMultiModeBacksideAutopilot::update()
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
 					_pos.vel_n * _pos.vel_n +
-					_pos.vy * _pos.vy +
+					_pos.vel_e * _pos.vel_e +
 					_pos.vel_d * _pos.vel_d));
 
 		// pitch channel -> rate of climb
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 5ade835ff..f139c25f4 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -108,14 +108,14 @@ private:
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	int		_control_task;			/**< task handle for sensor task */
 
-	int		_att_sub;				/**< vehicle attitude subscription */
-	int		_accel_sub;				/**< accelerometer subscription */
+	int		_att_sub;			/**< vehicle attitude subscription */
+	int		_accel_sub;			/**< accelerometer subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
 	int		_vcontrol_mode_sub;		/**< vehicle status subscription */
-	int 	_params_sub;			/**< notification of parameter updates */
-	int 	_manual_sub;			/**< notification of manual control updates */
+	int 		_params_sub;			/**< notification of parameter updates */
+	int 		_manual_sub;			/**< notification of manual control updates */
 	int		_global_pos_sub;		/**< global position subscription */
 
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
@@ -123,20 +123,19 @@ private:
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
 	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
 
-	struct vehicle_attitude_s				_att;			/**< vehicle attitude */
-	struct accel_report						_accel;			/**< body frame accelerations */
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct accel_report				_accel;			/**< body frame accelerations */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s						_airspeed;		/**< airspeed */
-	struct vehicle_control_mode_s			_vcontrol_mode;	/**< vehicle control mode */
+	struct airspeed_s				_airspeed;		/**< airspeed */
+	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
 	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
-	struct actuator_controls_s			_actuators_airframe;		/**< actuator control inputs */
-	struct vehicle_global_position_s		_global_pos;	/**< global position */
+	struct actuator_controls_s			_actuators_airframe;	/**< actuator control inputs */
+	struct vehicle_global_position_s		_global_pos;		/**< global position */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
-	bool		_airspeed_valid;		/**< flag if the airspeed measurement is valid */
 
 	struct {
 		float tconst;
@@ -245,7 +244,7 @@ private:
 	/**
 	 * Check for airspeed updates.
 	 */
-	bool		vehicle_airspeed_poll();
+	void		vehicle_airspeed_poll();
 
 	/**
 	 * Check for accel updates.
@@ -308,19 +307,18 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
 /* states */
-	_setpoint_valid(false),
-	_airspeed_valid(false)
+	_setpoint_valid(false)
 {
 	/* safely initialize structs */
-	_att = {0};
-	_accel = {0};
-	_att_sp = {0};
-	_manual = {0};
-	_airspeed = {0};
-	_vcontrol_mode = {0};
-	_actuators = {0};
-	_actuators_airframe = {0};
-	_global_pos = {0};
+	_att = {};
+	_accel = {};
+	_att_sp = {};
+	_manual = {};
+	_airspeed = {};
+	_vcontrol_mode = {};
+	_actuators = {};
+	_actuators_airframe = {};
+	_global_pos = {};
 
 
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
@@ -482,7 +480,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 	}
 }
 
-bool
+void
 FixedwingAttitudeControl::vehicle_airspeed_poll()
 {
 	/* check if there is a new position */
@@ -492,10 +490,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
 	if (airspeed_updated) {
 		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
 //		warnx("airspeed poll: ind: %.4f,  true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
-		return true;
 	}
-
-	return false;
 }
 
 void
@@ -569,7 +564,7 @@ FixedwingAttitudeControl::task_main()
 	parameters_update();
 
 	/* get an initial update for all sensor and status data */
-	(void)vehicle_airspeed_poll();
+	vehicle_airspeed_poll();
 	vehicle_setpoint_poll();
 	vehicle_accel_poll();
 	vehicle_control_mode_poll();
@@ -626,7 +621,7 @@ FixedwingAttitudeControl::task_main()
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
 
-			_airspeed_valid = vehicle_airspeed_poll();
+			vehicle_airspeed_poll();
 
 			vehicle_setpoint_poll();
 
@@ -666,9 +661,9 @@ FixedwingAttitudeControl::task_main()
 				float airspeed;
 
 				/* if airspeed is smaller than min, the sensor is not giving good readings */
-				if (!_airspeed_valid ||
-				    (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
-				    !isfinite(_airspeed.indicated_airspeed_m_s)) {
+				if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
+				    !isfinite(_airspeed.indicated_airspeed_m_s) ||
+				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
 					airspeed = _parameters.airspeed_trim;
 
 				} else {
@@ -791,10 +786,6 @@ FixedwingAttitudeControl::task_main()
 					warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp);
 				}
 
-				// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
-				// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
-				// 			_actuators.control[2], _actuators.control[3]);
-
 				/*
 				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
 				 * only once available
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4ee04d462..177f21667 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1031,6 +1031,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
 		raw.differential_pressure_counter++;
 
+		_airspeed.timestamp = hrt_absolute_time();
 		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
 		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f,
 					      raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index cb35a2541..ec2bc3a9a 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -60,3 +60,14 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
  * @group System
  */
 PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
+
+/**
+ * Set usage of IO board
+ *
+ * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
+ *
+ * @min 0
+ * @max 1
+ * @group System
+ */
+PARAM_DEFINE_INT32(SYS_USE_IO, 1);
diff --git a/src/modules/controllib/block/List.hpp b/src/modules/uORB/Publication.cpp
index 96b0b94d1..5a5981617 100644
--- a/src/modules/controllib/block/List.hpp
+++ b/src/modules/uORB/Publication.cpp
@@ -32,40 +32,49 @@
  ****************************************************************************/
 
 /**
- * @file Node.h
+ * @file Publication.cpp
  *
- * A node of a linked list.
  */
 
-#pragma once
+#include "Publication.hpp"
+#include "topics/vehicle_attitude.h"
+#include "topics/vehicle_local_position.h"
+#include "topics/vehicle_global_position.h"
+#include "topics/debug_key_value.h"
+#include "topics/actuator_controls.h"
+#include "topics/vehicle_global_velocity_setpoint.h"
+#include "topics/vehicle_attitude_setpoint.h"
+#include "topics/vehicle_rates_setpoint.h"
+#include "topics/actuator_outputs.h"
+#include "topics/encoders.h"
+
+namespace uORB {
+
+template<class T>
+Publication<T>::Publication(
+	List<PublicationBase *> * list,
+	const struct orb_metadata *meta) :
+	T(), // initialize data structure to zero
+	PublicationBase(list, meta) {
+}
 
 template<class T>
-class __EXPORT ListNode
-{
-public:
-	ListNode() : _sibling(NULL) {
-	}
-	void setSibling(T sibling) { _sibling = sibling; }
-	T getSibling() { return _sibling; }
-	T get() {
-		return _sibling;
-	}
-protected:
-	T _sibling;
-};
+Publication<T>::~Publication() {}
 
 template<class T>
-class __EXPORT List
-{
-public:
-	List() : _head() {
-	}
-	void add(T newNode) {
-		newNode->setSibling(getHead());
-		setHead(newNode);
-	}
-	T getHead() { return _head; }
-private:
-	void setHead(T &head) { _head = head; }
-	T _head;
-};
+void * Publication<T>::getDataVoidPtr() {
+	return (void *)(T *)(this);
+}
+
+template class __EXPORT Publication<vehicle_attitude_s>;
+template class __EXPORT Publication<vehicle_local_position_s>;
+template class __EXPORT Publication<vehicle_global_position_s>;
+template class __EXPORT Publication<debug_key_value_s>;
+template class __EXPORT Publication<actuator_controls_s>;
+template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
+template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
+template class __EXPORT Publication<vehicle_rates_setpoint_s>;
+template class __EXPORT Publication<actuator_outputs_s>;
+template class __EXPORT Publication<encoders_s>;
+
+}
diff --git a/src/modules/controllib/uorb/UOrbPublication.hpp b/src/modules/uORB/Publication.hpp
index 6f1f3fc1c..8650b3df8 100644
--- a/src/modules/controllib/uorb/UOrbPublication.hpp
+++ b/src/modules/uORB/Publication.hpp
@@ -32,32 +32,29 @@
  ****************************************************************************/
 
 /**
- * @file UOrbPublication.h
+ * @file Publication.h
  *
  */
 
 #pragma once
 
 #include <uORB/uORB.h>
-#include "../block/Block.hpp"
-#include "../block/List.hpp"
+#include <containers/List.hpp>
 
 
-namespace control
+namespace uORB
 {
 
-class Block;
-
 /**
  * Base publication warapper class, used in list traversal
  * of various publications.
  */
-class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *>
+class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *>
 {
 public:
 
-	UOrbPublicationBase(
-		List<UOrbPublicationBase *> * list,
+	PublicationBase(
+		List<PublicationBase *> * list,
 		const struct orb_metadata *meta) :
 		_meta(meta),
 		_handle(-1) {
@@ -71,7 +68,7 @@ public:
 		}
 	}
 	virtual void *getDataVoidPtr() = 0;
-	virtual ~UOrbPublicationBase() {
+	virtual ~PublicationBase() {
 		orb_unsubscribe(getHandle());
 	}
 	const struct orb_metadata *getMeta() { return _meta; }
@@ -83,12 +80,12 @@ protected:
 };
 
 /**
- * UOrb Publication wrapper class
+ * Publication wrapper class
  */
 template<class T>
-class UOrbPublication :
+class Publication :
 	public T, // this must be first!
-	public UOrbPublicationBase
+	public PublicationBase
 {
 public:
 	/**
@@ -98,13 +95,9 @@ public:
 	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
 	 *			for the topic.
 	 */
-	UOrbPublication(
-		List<UOrbPublicationBase *> * list,
-		const struct orb_metadata *meta) :
-		T(), // initialize data structure to zero
-		UOrbPublicationBase(list, meta) {
-	}
-	virtual ~UOrbPublication() {}
+	Publication(List<PublicationBase *> * list,
+		const struct orb_metadata *meta);
+	virtual ~Publication();
 	/*
 	 * XXX
 	 * This function gets the T struct, assuming
@@ -112,7 +105,7 @@ public:
 	 * should use dynamic cast, but doesn't
 	 * seem to be available
 	 */
-	void *getDataVoidPtr() { return (void *)(T *)(this); }
+	void *getDataVoidPtr();
 };
 
-} // namespace control
+} // namespace uORB
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
new file mode 100644
index 000000000..c1d1a938f
--- /dev/null
+++ b/src/modules/uORB/Subscription.cpp
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Subscription.cpp
+ *
+ */
+
+#include "Subscription.hpp"
+#include "topics/parameter_update.h"
+#include "topics/actuator_controls.h"
+#include "topics/vehicle_gps_position.h"
+#include "topics/sensor_combined.h"
+#include "topics/vehicle_attitude.h"
+#include "topics/vehicle_global_position.h"
+#include "topics/encoders.h"
+#include "topics/position_setpoint_triplet.h"
+#include "topics/vehicle_status.h"
+#include "topics/manual_control_setpoint.h"
+#include "topics/vehicle_local_position_setpoint.h"
+#include "topics/vehicle_local_position.h"
+#include "topics/vehicle_attitude_setpoint.h"
+#include "topics/vehicle_rates_setpoint.h"
+
+namespace uORB
+{
+
+bool __EXPORT SubscriptionBase::updated()
+{
+	bool isUpdated = false;
+	orb_check(_handle, &isUpdated);
+	return isUpdated;
+}
+
+template<class T>
+Subscription<T>::Subscription(
+	List<SubscriptionBase *> * list,
+	const struct orb_metadata *meta, unsigned interval) :
+	T(), // initialize data structure to zero
+	SubscriptionBase(list, meta) {
+	setHandle(orb_subscribe(getMeta()));
+	orb_set_interval(getHandle(), interval);
+}
+
+template<class T>
+Subscription<T>::~Subscription() {}
+
+template<class T>
+void * Subscription<T>::getDataVoidPtr() {
+	return (void *)(T *)(this);
+}
+
+template<class T>
+T Subscription<T>::getData() {
+	return T(*this);
+}
+
+template class __EXPORT Subscription<parameter_update_s>;
+template class __EXPORT Subscription<actuator_controls_s>;
+template class __EXPORT Subscription<vehicle_gps_position_s>;
+template class __EXPORT Subscription<sensor_combined_s>;
+template class __EXPORT Subscription<vehicle_attitude_s>;
+template class __EXPORT Subscription<vehicle_global_position_s>;
+template class __EXPORT Subscription<encoders_s>;
+template class __EXPORT Subscription<position_setpoint_triplet_s>;
+template class __EXPORT Subscription<vehicle_status_s>;
+template class __EXPORT Subscription<manual_control_setpoint_s>;
+template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
+template class __EXPORT Subscription<vehicle_local_position_s>;
+template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
+template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
+
+} // namespace uORB
diff --git a/src/modules/controllib/uorb/UOrbSubscription.hpp b/src/modules/uORB/Subscription.hpp
index d337d89a8..34e9a83e0 100644
--- a/src/modules/controllib/uorb/UOrbSubscription.hpp
+++ b/src/modules/uORB/Subscription.hpp
@@ -32,28 +32,25 @@
  ****************************************************************************/
 
 /**
- * @file UOrbSubscription.h
+ * @file Subscription.h
  *
  */
 
 #pragma once
 
 #include <uORB/uORB.h>
-#include "../block/Block.hpp"
-#include "../block/List.hpp"
+#include <containers/List.hpp>
 
 
-namespace control
+namespace uORB
 {
 
-class Block;
-
 /**
  * Base subscription warapper class, used in list traversal
  * of various subscriptions.
  */
-class __EXPORT UOrbSubscriptionBase :
-	public ListNode<control::UOrbSubscriptionBase *>
+class __EXPORT SubscriptionBase :
+	public ListNode<SubscriptionBase *>
 {
 public:
 // methods
@@ -64,8 +61,8 @@ public:
 	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
 	 *			for the topic.
 	 */
-	UOrbSubscriptionBase(
-		List<UOrbSubscriptionBase *> * list,
+	SubscriptionBase(
+		List<SubscriptionBase *> * list,
 		const struct orb_metadata *meta) :
 		_meta(meta),
 		_handle() {
@@ -78,7 +75,7 @@ public:
 		}
 	}
 	virtual void *getDataVoidPtr() = 0;
-	virtual ~UOrbSubscriptionBase() {
+	virtual ~SubscriptionBase() {
 		orb_unsubscribe(_handle);
 	}
 // accessors
@@ -93,12 +90,12 @@ protected:
 };
 
 /**
- * UOrb Subscription wrapper class
+ * Subscription wrapper class
  */
 template<class T>
-class __EXPORT UOrbSubscription :
+class __EXPORT Subscription :
 	public T, // this must be first!
-	public UOrbSubscriptionBase
+	public SubscriptionBase
 {
 public:
 	/**
@@ -109,19 +106,13 @@ public:
 	 *			for the topic.
 	 * @param interval  The minimum interval in milliseconds between updates
 	 */
-	UOrbSubscription(
-		List<UOrbSubscriptionBase *> * list,
-		const struct orb_metadata *meta, unsigned interval) :
-		T(), // initialize data structure to zero
-		UOrbSubscriptionBase(list, meta) {
-		setHandle(orb_subscribe(getMeta()));
-		orb_set_interval(getHandle(), interval);
-	}
-
+	Subscription(
+		List<SubscriptionBase *> * list,
+		const struct orb_metadata *meta, unsigned interval);
 	/**
 	 * Deconstructor
 	 */
-	virtual ~UOrbSubscription() {}
+	virtual ~Subscription();
 
 	/*
 	 * XXX
@@ -130,8 +121,8 @@ public:
 	 * should use dynamic cast, but doesn't
 	 * seem to be available
 	 */
-	void *getDataVoidPtr() { return (void *)(T *)(this); }
-	T getData() { return T(*this); }
+	void *getDataVoidPtr();
+	T getData();
 };
 
-} // namespace control
+} // namespace uORB
diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk
index 5ec31ab01..0c29101fe 100644
--- a/src/modules/uORB/module.mk
+++ b/src/modules/uORB/module.mk
@@ -41,4 +41,6 @@ MODULE_COMMAND		= uorb
 MODULE_STACKSIZE	= 4096
 
 SRCS			= uORB.cpp \
-			  objects_common.cpp
+			  objects_common.cpp \
+			  Publication.cpp \
+			  Subscription.cpp
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 4c84c1f25..fb24de8d1 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -190,3 +190,6 @@ ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
 
 #include "topics/esc_status.h"
 ORB_DEFINE(esc_status, struct esc_status_s);
+
+#include "topics/encoders.h"
+ORB_DEFINE(encoders, struct encoders_s);
diff --git a/src/modules/controllib/uorb/UOrbSubscription.cpp b/src/modules/uORB/topics/encoders.h
index 022cadd24..588c0ddb1 100644
--- a/src/modules/controllib/uorb/UOrbSubscription.cpp
+++ b/src/modules/uORB/topics/encoders.h
@@ -32,20 +32,35 @@
  ****************************************************************************/
 
 /**
- * @file UOrbSubscription.cpp
+ * @file encoders.h
+ *
+ * Encoders topic.
  *
  */
 
-#include "UOrbSubscription.hpp"
+#ifndef TOPIC_ENCODERS_H
+#define TOPIC_ENCODERS_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
 
-namespace control
-{
+#define NUM_ENCODERS 4
+
+struct encoders_s {
+	uint64_t timestamp;
+	int64_t counts[NUM_ENCODERS]; // counts of encoder
+	float velocity[NUM_ENCODERS]; // counts of encoder/ second
+};
+
+/**
+ * @}
+ */
 
-bool __EXPORT UOrbSubscriptionBase::updated()
-{
-	bool isUpdated = false;
-	orb_check(_handle, &isUpdated);
-	return isUpdated;
-}
+ORB_DECLARE(encoders);
 
-} // namespace control
+#endif