Added control library.

17 files changed, 2983 insertions, 0 deletions

diff --git a/apps/examples/control_demo/Makefile b/apps/examples/control_demo/Makefile
new file mode 100644
index 000000000..6e40e645f
--- /dev/null
+++ b/apps/examples/control_demo/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Basic example application
+#
+
+APPNAME		 = control_demo
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/examples/control_demo/control_demo.cpp b/apps/examples/control_demo/control_demo.cpp
new file mode 100644
index 000000000..c60c5e8b9
--- /dev/null
+++ b/apps/examples/control_demo/control_demo.cpp
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Example User <mail@example.com>
+ *
+ * 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 control_demo.cpp
+ * Demonstration of control library
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/control/fixedwing.hpp>
+#include <systemlib/param/param.h>
+#include <drivers/drv_hrt.h>
+#include <math.h>
+
+static bool thread_should_exit = false;     /**< Deamon exit flag */
+static bool thread_running = false;     /**< Deamon status flag */
+static int deamon_task;             /**< Handle of deamon task / thread */
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int control_demo_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int control_demo_thread_main(int argc, char *argv[]);
+
+/**
+ * Test function
+ */
+void test();
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: control_demo {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int control_demo_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("control_demo already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("control_demo",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 10,
+					 4096,
+					 control_demo_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "test")) {
+		test();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tcontrol_demo app is running\n");
+
+		} else {
+			printf("\tcontrol_demo app not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int control_demo_thread_main(int argc, char *argv[])
+{
+
+	printf("[control_Demo] starting\n");
+
+	using namespace control;
+
+	fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
+
+	thread_running = true;
+
+	while (!thread_should_exit) {
+		autopilot.update();
+	}
+
+	printf("[control_demo] exiting.\n");
+
+	thread_running = false;
+
+	return 0;
+}
+
+void test()
+{
+	printf("beginning control lib test\n");
+	control::basicBlocksTest();
+}
diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
new file mode 100644
index 000000000..4eec456fb
--- /dev/null
+++ b/apps/examples/control_demo/params.c
@@ -0,0 +1,63 @@
+#include <systemlib/param/param.h>
+
+// currently tuned for easystar from arkhangar in HIL
+//https://github.com/arktools/arkhangar
+
+// 16 is max name length
+
+// gyro low pass filter
+PARAM_DEFINE_FLOAT(FWB_P_LP, 10.0f); // roll rate low pass cut freq
+PARAM_DEFINE_FLOAT(FWB_Q_LP, 10.0f); // pitch rate low pass cut freq
+PARAM_DEFINE_FLOAT(FWB_R_LP, 10.0f); // yaw rate low pass cut freq
+
+// yaw washout
+PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
+
+// stabilization mode
+PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.1f); // roll rate 2 aileron
+PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
+PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
+
+//  psi -> phi -> p
+PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 2.0f);      // heading 2 roll
+PARAM_DEFINE_FLOAT(FWB_PHI2P, 2.0f);        // roll to roll rate
+PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 1.0f);  // roll limit
+
+// velocity -> theta
+PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.5f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_THE_MIN, -1.0f);
+PARAM_DEFINE_FLOAT(FWB_THE_MAX, 1.0f);
+
+
+// theta -> q
+PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
+
+//  h -> thr
+PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.005f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.001f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.01f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 250.0f);
+
+// crosstrack
+PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.01f);
+
+// speed command
+PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
+PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);
+PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
+
+// trim
+PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.7f);
diff --git a/apps/systemlib/control/block/Block.cpp b/apps/systemlib/control/block/Block.cpp
new file mode 100644
index 000000000..5994d2315
--- /dev/null
+++ b/apps/systemlib/control/block/Block.cpp
@@ -0,0 +1,210 @@
+/****************************************************************************
+ *
+ *   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 Block.cpp
+ *
+ * Controller library code
+ */
+
+#include <math.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "Block.hpp"
+#include "BlockParam.hpp"
+#include "UOrbSubscription.hpp"
+#include "UOrbPublication.hpp"
+
+namespace control
+{
+
+Block::Block(SuperBlock *parent, const char *name) :
+	_name(name),
+	_parent(parent),
+	_dt(0),
+	_subscriptions(),
+	_params()
+{
+	if (getParent() != NULL) {
+		getParent()->getChildren().add(this);
+	}
+}
+
+void Block::getName(char *buf, size_t n)
+{
+	if (getParent() == NULL) {
+		strncpy(buf, _name, n);
+
+	} else {
+		char parentName[blockNameLengthMax];
+		getParent()->getName(parentName, n);
+
+		if (!strcmp(_name, "")) {
+			strncpy(buf, parentName, blockNameLengthMax);
+
+		} else {
+			snprintf(buf, blockNameLengthMax, "%s_%s", parentName, _name);
+		}
+	}
+}
+
+void Block::updateParams()
+{
+	BlockParamBase *param = getParams().getHead();
+	int count = 0;
+
+	while (param != NULL) {
+		if (count++ > maxParamsPerBlock) {
+			char name[blockNameLengthMax];
+			getName(name, blockNameLengthMax);
+			printf("exceeded max params for block: %s\n", name);
+			break;
+		}
+
+		//printf("updating param: %s\n", param->getName());
+		param->update();
+		param = param->getSibling();
+	}
+}
+
+void Block::updateSubscriptions()
+{
+	UOrbSubscriptionBase *sub = getSubscriptions().getHead();
+	int count = 0;
+
+	while (sub != NULL) {
+		if (count++ > maxSubscriptionsPerBlock) {
+			char name[blockNameLengthMax];
+			getName(name, blockNameLengthMax);
+			printf("exceeded max subscriptions for block: %s\n", name);
+			break;
+		}
+
+		sub->update();
+		sub = sub->getSibling();
+	}
+}
+
+void Block::updatePublications()
+{
+	UOrbPublicationBase *pub = getPublications().getHead();
+	int count = 0;
+
+	while (pub != NULL) {
+		if (count++ > maxPublicationsPerBlock) {
+			char name[blockNameLengthMax];
+			getName(name, blockNameLengthMax);
+			printf("exceeded max publications for block: %s\n", name);
+			break;
+		}
+
+		pub->update();
+		pub = pub->getSibling();
+	}
+}
+
+void SuperBlock::setDt(float dt)
+{
+	Block::setDt(dt);
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->setDt(dt);
+		child = child->getSibling();
+	}
+}
+
+void SuperBlock::updateChildParams()
+{
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->updateParams();
+		child = child->getSibling();
+	}
+}
+
+void SuperBlock::updateChildSubscriptions()
+{
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->updateSubscriptions();
+		child = child->getSibling();
+	}
+}
+
+void SuperBlock::updateChildPublications()
+{
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->updatePublications();
+		child = child->getSibling();
+	}
+}
+
+} // namespace control
diff --git a/apps/systemlib/control/block/Block.hpp b/apps/systemlib/control/block/Block.hpp
new file mode 100644
index 000000000..258701f27
--- /dev/null
+++ b/apps/systemlib/control/block/Block.hpp
@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   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 Block.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <inttypes.h>
+
+#include "List.hpp"
+
+namespace control
+{
+
+static const uint16_t maxChildrenPerBlock = 100;
+static const uint16_t maxParamsPerBlock = 100;
+static const uint16_t maxSubscriptionsPerBlock = 100;
+static const uint16_t maxPublicationsPerBlock = 100;
+static const uint8_t blockNameLengthMax = 80;
+
+// forward declaration
+class BlockParamBase;
+class UOrbSubscriptionBase;
+class UOrbPublicationBase;
+class SuperBlock;
+
+/**
+ */
+class __EXPORT Block :
+	public ListNode<Block *>
+{
+public:
+	friend class BlockParamBase;
+// methods
+	Block(SuperBlock *parent, const char *name);
+	void getName(char *name, size_t n);
+	virtual ~Block() {};
+	virtual void updateParams();
+	virtual void updateSubscriptions();
+	virtual void updatePublications();
+	virtual void setDt(float dt) { _dt = dt; }
+// accessors
+	float getDt() { return _dt; }
+protected:
+// accessors
+	SuperBlock *getParent() { return _parent; }
+	List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
+	List<UOrbPublicationBase *> & getPublications() { return _publications; }
+	List<BlockParamBase *> & getParams() { return _params; }
+// attributes
+	const char *_name;
+	SuperBlock *_parent;
+	float _dt;
+	List<UOrbSubscriptionBase *> _subscriptions;
+	List<UOrbPublicationBase *> _publications;
+	List<BlockParamBase *> _params;
+};
+
+class __EXPORT SuperBlock :
+	public Block
+{
+public:
+	friend class Block;
+// methods
+	SuperBlock(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_children() {
+	}
+	virtual ~SuperBlock() {};
+	virtual void setDt(float dt);
+	virtual void updateParams() {
+		Block::updateParams();
+
+		if (getChildren().getHead() != NULL) updateChildParams();
+	}
+	virtual void updateSubscriptions() {
+		Block::updateSubscriptions();
+
+		if (getChildren().getHead() != NULL) updateChildSubscriptions();
+	}
+	virtual void updatePublications() {
+		Block::updatePublications();
+
+		if (getChildren().getHead() != NULL) updateChildPublications();
+	}
+protected:
+// methods
+	List<Block *> & getChildren() { return _children; }
+	void updateChildParams();
+	void updateChildSubscriptions();
+	void updateChildPublications();
+// attributes
+	List<Block *> _children;
+};
+
+} // namespace control
diff --git a/apps/systemlib/control/block/BlockParam.cpp b/apps/systemlib/control/block/BlockParam.cpp
new file mode 100644
index 000000000..b3f49f7db
--- /dev/null
+++ b/apps/systemlib/control/block/BlockParam.cpp
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   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 Blockparam.cpp
+ *
+ * Controller library code
+ */
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "BlockParam.hpp"
+
+namespace control
+{
+
+BlockParamBase::BlockParamBase(Block *parent, const char *name) :
+	_handle(PARAM_INVALID)
+{
+	char fullname[blockNameLengthMax];
+
+	if (parent == NULL) {
+		strncpy(fullname, name, blockNameLengthMax);
+
+	} else {
+		char parentName[blockNameLengthMax];
+		parent->getName(parentName, blockNameLengthMax);
+
+		if (!strcmp(name, "")) {
+			strncpy(fullname, parentName, blockNameLengthMax);
+
+		} else {
+			snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
+		}
+
+		parent->getParams().add(this);
+	}
+
+	_handle = param_find(fullname);
+
+	if (_handle == PARAM_INVALID)
+		printf("error finding param: %s\n", fullname);
+};
+
+} // namespace control
diff --git a/apps/systemlib/control/block/BlockParam.hpp b/apps/systemlib/control/block/BlockParam.hpp
new file mode 100644
index 000000000..7f86d1717
--- /dev/null
+++ b/apps/systemlib/control/block/BlockParam.hpp
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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 BlockParam.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <systemlib/param/param.h>
+
+#include "Block.hpp"
+#include "List.hpp"
+
+namespace control
+{
+
+/**
+ * A base class for block params that enables traversing linked list.
+ */
+class __EXPORT BlockParamBase : public ListNode<BlockParamBase *>
+{
+public:
+	BlockParamBase(Block *parent, const char *name);
+	virtual ~BlockParamBase() {};
+	virtual void update() = 0;
+	const char *getName() { return param_name(_handle); }
+protected:
+	param_t _handle;
+};
+
+/**
+ * Parameters that are tied to blocks for updating and nameing.
+ */
+template<class T>
+class __EXPORT BlockParam : public BlockParamBase
+{
+public:
+	BlockParam(Block *block, const char *name) :
+		BlockParamBase(block, name),
+		_val() {
+		update();
+	}
+	T get() { return _val; }
+	void set(T val) { _val = val; }
+	void update() {
+		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
+	}
+protected:
+	T _val;
+};
+
+} // namespace control
diff --git a/apps/systemlib/control/block/List.hpp b/apps/systemlib/control/block/List.hpp
new file mode 100644
index 000000000..96b0b94d1
--- /dev/null
+++ b/apps/systemlib/control/block/List.hpp
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   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 Node.h
+ *
+ * A node of a linked list.
+ */
+
+#pragma once
+
+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;
+};
+
+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;
+};
diff --git a/apps/systemlib/control/block/UOrbPublication.cpp b/apps/systemlib/control/block/UOrbPublication.cpp
new file mode 100644
index 000000000..f69b39d90
--- /dev/null
+++ b/apps/systemlib/control/block/UOrbPublication.cpp
@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   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/apps/systemlib/control/block/UOrbPublication.hpp b/apps/systemlib/control/block/UOrbPublication.hpp
new file mode 100644
index 000000000..a36f4429f
--- /dev/null
+++ b/apps/systemlib/control/block/UOrbPublication.hpp
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   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.h
+ *
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include "Block.hpp"
+#include "List.hpp"
+
+
+namespace control
+{
+
+class Block;
+
+/**
+ * Base publication warapper class, used in list traversal
+ * of various publications.
+ */
+class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *>
+{
+public:
+
+	UOrbPublicationBase(
+		List<UOrbPublicationBase *> * list,
+		const struct orb_metadata *meta) :
+		_meta(meta),
+		_handle() {
+		if (list != NULL) list->add(this);
+	}
+	void update() {
+		orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+	}
+	virtual void *getDataVoidPtr() = 0;
+	virtual ~UOrbPublicationBase() {
+		orb_unsubscribe(getHandle());
+	}
+	const struct orb_metadata *getMeta() { return _meta; }
+	int getHandle() { return _handle; }
+protected:
+	void setHandle(orb_advert_t handle) { _handle = handle; }
+	const struct orb_metadata *_meta;
+	orb_advert_t _handle;
+};
+
+/**
+ * UOrb Publication wrapper class
+ */
+template<class T>
+class UOrbPublication :
+	public T, // this must be first!
+	public UOrbPublicationBase
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param list      A list interface for adding to list during construction
+	 * @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) {
+		// It is important that we call T()
+		// before we publish the data, so we
+		// call this here instead of the base class
+		setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
+	}
+	virtual ~UOrbPublication() {}
+	/*
+	 * 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() { return (void *)(T *)(this); }
+};
+
+} // namespace control
diff --git a/apps/systemlib/control/block/UOrbSubscription.cpp b/apps/systemlib/control/block/UOrbSubscription.cpp
new file mode 100644
index 000000000..022cadd24
--- /dev/null
+++ b/apps/systemlib/control/block/UOrbSubscription.cpp
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   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 UOrbSubscription.cpp
+ *
+ */
+
+#include "UOrbSubscription.hpp"
+
+namespace control
+{
+
+bool __EXPORT UOrbSubscriptionBase::updated()
+{
+	bool isUpdated = false;
+	orb_check(_handle, &isUpdated);
+	return isUpdated;
+}
+
+} // namespace control
diff --git a/apps/systemlib/control/block/UOrbSubscription.hpp b/apps/systemlib/control/block/UOrbSubscription.hpp
new file mode 100644
index 000000000..22cc2e114
--- /dev/null
+++ b/apps/systemlib/control/block/UOrbSubscription.hpp
@@ -0,0 +1,137 @@
+/****************************************************************************
+ *
+ *   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 UOrbSubscription.h
+ *
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include "Block.hpp"
+#include "List.hpp"
+
+
+namespace control
+{
+
+class Block;
+
+/**
+ * Base subscription warapper class, used in list traversal
+ * of various subscriptions.
+ */
+class __EXPORT UOrbSubscriptionBase :
+	public ListNode<control::UOrbSubscriptionBase *>
+{
+public:
+// methods
+
+	/**
+	 * Constructor
+	 *
+	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
+	 *			for the topic.
+	 */
+	UOrbSubscriptionBase(
+		List<UOrbSubscriptionBase *> * list,
+		const struct orb_metadata *meta) :
+		_meta(meta),
+		_handle() {
+		if (list != NULL) list->add(this);
+	}
+	bool updated();
+	void update() {
+		if (updated()) {
+			orb_copy(_meta, _handle, getDataVoidPtr());
+		}
+	}
+	virtual void *getDataVoidPtr() = 0;
+	virtual ~UOrbSubscriptionBase() {
+		orb_unsubscribe(_handle);
+	}
+// accessors
+	const struct orb_metadata *getMeta() { return _meta; }
+	int getHandle() { return _handle; }
+protected:
+// accessors
+	void setHandle(int handle) { _handle = handle; }
+// attributes
+	const struct orb_metadata *_meta;
+	int _handle;
+};
+
+/**
+ * UOrb Subscription wrapper class
+ */
+template<class T>
+class __EXPORT UOrbSubscription :
+	public T, // this must be first!
+	public UOrbSubscriptionBase
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param list      A list interface for adding to list during construction
+	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
+	 *			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);
+	}
+
+	/**
+	 * Deconstructor
+	 */
+	virtual ~UOrbSubscription() {}
+
+	/*
+	 * 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() { return (void *)(T *)(this); }
+	T getData() { return T(*this); }
+};
+
+} // namespace control
diff --git a/apps/systemlib/control/blocks.cpp b/apps/systemlib/control/blocks.cpp
new file mode 100644
index 000000000..c6c374300
--- /dev/null
+++ b/apps/systemlib/control/blocks.cpp
@@ -0,0 +1,486 @@
+/****************************************************************************
+ *
+ *   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 blocks.cpp
+ *
+ * Controller library code
+ */
+
+#include <math.h>
+#include <stdio.h>
+
+#include "blocks.hpp"
+
+namespace control
+{
+
+int basicBlocksTest()
+{
+	blockLimitTest();
+	blockLimitSymTest();
+	blockLowPassTest();
+	blockHighPassTest();
+	blockIntegralTest();
+	blockIntegralTrapTest();
+	blockDerivativeTest();
+	blockPTest();
+	blockPITest();
+	blockPDTest();
+	blockPIDTest();
+	blockOutputTest();
+	blockRandUniformTest();
+	blockRandGaussTest();
+	return 0;
+}
+
+float BlockLimit::update(float input)
+{
+	if (input > getMax()) {
+		input = _max.get();
+
+	} else if (input < getMin()) {
+		input = getMin();
+	}
+
+	return input;
+}
+
+int blockLimitTest()
+{
+	printf("Test BlockLimit\t\t\t: ");
+	BlockLimit limit(NULL, "TEST");
+	// initial state
+	ASSERT(equal(1.0f, limit.getMax()));
+	ASSERT(equal(-1.0f, limit.getMin()));
+	ASSERT(equal(0.0f, limit.getDt()));
+	// update
+	ASSERT(equal(-1.0f, limit.update(-2.0f)));
+	ASSERT(equal(1.0f, limit.update(2.0f)));
+	ASSERT(equal(0.0f, limit.update(0.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockLimitSym::update(float input)
+{
+	if (input > getMax()) {
+		input = _max.get();
+
+	} else if (input < -getMax()) {
+		input = -getMax();
+	}
+
+	return input;
+}
+
+int blockLimitSymTest()
+{
+	printf("Test BlockLimitSym\t\t: ");
+	BlockLimitSym limit(NULL, "TEST");
+	// initial state
+	ASSERT(equal(1.0f, limit.getMax()));
+	ASSERT(equal(0.0f, limit.getDt()));
+	// update
+	ASSERT(equal(-1.0f, limit.update(-2.0f)));
+	ASSERT(equal(1.0f, limit.update(2.0f)));
+	ASSERT(equal(0.0f, limit.update(0.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockLowPass::update(float input)
+{
+	float b = 2 * float(M_PI) * getFCut() * getDt();
+	float a = b / (1 + b);
+	setState(a * input + (1 - a)*getState());
+	return getState();
+}
+
+int blockLowPassTest()
+{
+	printf("Test BlockLowPass\t\t: ");
+	BlockLowPass lowPass(NULL, "TEST_LP");
+	// test initial state
+	ASSERT(equal(10.0f, lowPass.getFCut()));
+	ASSERT(equal(0.0f, lowPass.getState()));
+	ASSERT(equal(0.0f, lowPass.getDt()));
+	// set dt
+	lowPass.setDt(0.1f);
+	ASSERT(equal(0.1f, lowPass.getDt()));
+	// set state
+	lowPass.setState(1.0f);
+	ASSERT(equal(1.0f, lowPass.getState()));
+	// test update
+	ASSERT(equal(1.8626974f, lowPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		lowPass.update(2.0f);
+	}
+
+	ASSERT(equal(2.0f, lowPass.getState()));
+	ASSERT(equal(2.0f, lowPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+};
+
+float BlockHighPass::update(float input)
+{
+	float b = 2 * float(M_PI) * getFCut() * getDt();
+	float a = 1 / (1 + b);
+	setY(a * (getY() + input - getU()));
+	setU(input);
+	return getY();
+}
+
+int blockHighPassTest()
+{
+	printf("Test BlockHighPass\t\t: ");
+	BlockHighPass highPass(NULL, "TEST_HP");
+	// test initial state
+	ASSERT(equal(10.0f, highPass.getFCut()));
+	ASSERT(equal(0.0f, highPass.getU()));
+	ASSERT(equal(0.0f, highPass.getY()));
+	ASSERT(equal(0.0f, highPass.getDt()));
+	// set dt
+	highPass.setDt(0.1f);
+	ASSERT(equal(0.1f, highPass.getDt()));
+	// set state
+	highPass.setU(1.0f);
+	ASSERT(equal(1.0f, highPass.getU()));
+	highPass.setY(1.0f);
+	ASSERT(equal(1.0f, highPass.getY()));
+	// test update
+	ASSERT(equal(0.2746051f, highPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		highPass.update(2.0f);
+	}
+
+	ASSERT(equal(0.0f, highPass.getY()));
+	ASSERT(equal(0.0f, highPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockIntegral::update(float input)
+{
+	// trapezoidal integration
+	setY(_limit.update(getY() + input * getDt()));
+	return getY();
+}
+
+int blockIntegralTest()
+{
+	printf("Test BlockIntegral\t\t: ");
+	BlockIntegral integral(NULL, "TEST_I");
+	// test initial state
+	ASSERT(equal(1.0f, integral.getMax()));
+	ASSERT(equal(0.0f, integral.getDt()));
+	// set dt
+	integral.setDt(0.1f);
+	ASSERT(equal(0.1f, integral.getDt()));
+	// set Y
+	integral.setY(0.9f);
+	ASSERT(equal(0.9f, integral.getY()));
+
+	// test exceed max
+	for (int i = 0; i < 100; i++) {
+		integral.update(1.0f);
+	}
+
+	ASSERT(equal(1.0f, integral.update(1.0f)));
+	// test exceed min
+	integral.setY(-0.9f);
+	ASSERT(equal(-0.9f, integral.getY()));
+
+	for (int i = 0; i < 100; i++) {
+		integral.update(-1.0f);
+	}
+
+	ASSERT(equal(-1.0f, integral.update(-1.0f)));
+	// test update
+	integral.setY(0.1f);
+	ASSERT(equal(0.2f, integral.update(1.0)));
+	ASSERT(equal(0.2f, integral.getY()));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockIntegralTrap::update(float input)
+{
+	// trapezoidal integration
+	setY(_limit.update(getY() +
+			   (getU() + input) / 2.0f * getDt()));
+	setU(input);
+	return getY();
+}
+
+int blockIntegralTrapTest()
+{
+	printf("Test BlockIntegralTrap\t\t: ");
+	BlockIntegralTrap integral(NULL, "TEST_I");
+	// test initial state
+	ASSERT(equal(1.0f, integral.getMax()));
+	ASSERT(equal(0.0f, integral.getDt()));
+	// set dt
+	integral.setDt(0.1f);
+	ASSERT(equal(0.1f, integral.getDt()));
+	// set U
+	integral.setU(1.0f);
+	ASSERT(equal(1.0f, integral.getU()));
+	// set Y
+	integral.setY(0.9f);
+	ASSERT(equal(0.9f, integral.getY()));
+
+	// test exceed max
+	for (int i = 0; i < 100; i++) {
+		integral.update(1.0f);
+	}
+
+	ASSERT(equal(1.0f, integral.update(1.0f)));
+	// test exceed min
+	integral.setU(-1.0f);
+	integral.setY(-0.9f);
+	ASSERT(equal(-0.9f, integral.getY()));
+
+	for (int i = 0; i < 100; i++) {
+		integral.update(-1.0f);
+	}
+
+	ASSERT(equal(-1.0f, integral.update(-1.0f)));
+	// test update
+	integral.setU(2.0f);
+	integral.setY(0.1f);
+	ASSERT(equal(0.25f, integral.update(1.0)));
+	ASSERT(equal(0.25f, integral.getY()));
+	ASSERT(equal(1.0f, integral.getU()));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockDerivative::update(float input)
+{
+	float output = _lowPass.update((input - getU()) / getDt());
+	setU(input);
+	return output;
+}
+
+int blockDerivativeTest()
+{
+	printf("Test BlockDerivative\t\t: ");
+	BlockDerivative derivative(NULL, "TEST_D");
+	// test initial state
+	ASSERT(equal(0.0f, derivative.getU()));
+	ASSERT(equal(10.0f, derivative.getLP()));
+	// set dt
+	derivative.setDt(0.1f);
+	ASSERT(equal(0.1f, derivative.getDt()));
+	// set U
+	derivative.setU(1.0f);
+	ASSERT(equal(1.0f, derivative.getU()));
+	// test  update
+	ASSERT(equal(8.6269744f, derivative.update(2.0f)));
+	ASSERT(equal(2.0f, derivative.getU()));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPTest()
+{
+	printf("Test BlockP\t\t\t: ");
+	BlockP blockP(NULL, "TEST_P");
+	// test initial state
+	ASSERT(equal(0.2f, blockP.getKP()));
+	ASSERT(equal(0.0f, blockP.getDt()));
+	// set dt
+	blockP.setDt(0.1f);
+	ASSERT(equal(0.1f, blockP.getDt()));
+	// test  update
+	ASSERT(equal(0.4f, blockP.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPITest()
+{
+	printf("Test BlockPI\t\t\t: ");
+	BlockPI blockPI(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.2f, blockPI.getKP()));
+	ASSERT(equal(0.1f, blockPI.getKI()));
+	ASSERT(equal(0.0f, blockPI.getDt()));
+	ASSERT(equal(1.0f, blockPI.getIntegral().getMax()));
+	// set dt
+	blockPI.setDt(0.1f);
+	ASSERT(equal(0.1f, blockPI.getDt()));
+	// set integral state
+	blockPI.getIntegral().setY(0.1f);
+	ASSERT(equal(0.1f, blockPI.getIntegral().getY()));
+	// test  update
+	// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43
+	ASSERT(equal(0.43f, blockPI.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPDTest()
+{
+	printf("Test BlockPD\t\t\t: ");
+	BlockPD blockPD(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.2f, blockPD.getKP()));
+	ASSERT(equal(0.01f, blockPD.getKD()));
+	ASSERT(equal(0.0f, blockPD.getDt()));
+	ASSERT(equal(10.0f, blockPD.getDerivative().getLP()));
+	// set dt
+	blockPD.setDt(0.1f);
+	ASSERT(equal(0.1f, blockPD.getDt()));
+	// set derivative state
+	blockPD.getDerivative().setU(1.0f);
+	ASSERT(equal(1.0f, blockPD.getDerivative().getU()));
+	// test  update
+	// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744
+	ASSERT(equal(0.486269744f, blockPD.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPIDTest()
+{
+	printf("Test BlockPID\t\t\t: ");
+	BlockPID blockPID(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.2f, blockPID.getKP()));
+	ASSERT(equal(0.1f, blockPID.getKI()));
+	ASSERT(equal(0.01f, blockPID.getKD()));
+	ASSERT(equal(0.0f, blockPID.getDt()));
+	ASSERT(equal(10.0f, blockPID.getDerivative().getLP()));
+	ASSERT(equal(1.0f, blockPID.getIntegral().getMax()));
+	// set dt
+	blockPID.setDt(0.1f);
+	ASSERT(equal(0.1f, blockPID.getDt()));
+	// set derivative state
+	blockPID.getDerivative().setU(1.0f);
+	ASSERT(equal(1.0f, blockPID.getDerivative().getU()));
+	// set integral state
+	blockPID.getIntegral().setY(0.1f);
+	ASSERT(equal(0.1f, blockPID.getIntegral().getY()));
+	// test  update
+	// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697
+	ASSERT(equal(0.5162697f, blockPID.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockOutputTest()
+{
+	printf("Test BlockOutput\t\t: ");
+	BlockOutput blockOutput(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.0f, blockOutput.getDt()));
+	ASSERT(equal(0.5f, blockOutput.get()));
+	ASSERT(equal(-1.0f, blockOutput.getMin()));
+	ASSERT(equal(1.0f, blockOutput.getMax()));
+	// test update below min
+	blockOutput.update(-2.0f);
+	ASSERT(equal(-1.0f, blockOutput.get()));
+	// test update above max
+	blockOutput.update(2.0f);
+	ASSERT(equal(1.0f, blockOutput.get()));
+	// test trim
+	blockOutput.update(0.0f);
+	ASSERT(equal(0.5f, blockOutput.get()));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockRandUniformTest()
+{
+	srand(1234);
+	printf("Test BlockRandUniform\t\t: ");
+	BlockRandUniform blockRandUniform(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.0f, blockRandUniform.getDt()));
+	ASSERT(equal(-1.0f, blockRandUniform.getMin()));
+	ASSERT(equal(1.0f, blockRandUniform.getMax()));
+	// test update
+	int n = 10000;
+	float mean = blockRandUniform.update();
+
+	// recursive mean algorithm from Knuth
+	for (int i = 2; i < n + 1; i++) {
+		float val = blockRandUniform.update();
+		mean += (val - mean) / i;
+		ASSERT(val <= blockRandUniform.getMax());
+		ASSERT(val >= blockRandUniform.getMin());
+	}
+
+	ASSERT(equal(mean, (blockRandUniform.getMin() +
+			    blockRandUniform.getMax()) / 2, 1e-1));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockRandGaussTest()
+{
+	srand(1234);
+	printf("Test BlockRandGauss\t\t: ");
+	BlockRandGauss blockRandGauss(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.0f, blockRandGauss.getDt()));
+	ASSERT(equal(1.0f, blockRandGauss.getMean()));
+	ASSERT(equal(2.0f, blockRandGauss.getStdDev()));
+	// test update
+	int n = 10000;
+	float mean = blockRandGauss.update();
+	float sum = 0;
+
+	// recursive mean, stdev algorithm from Knuth
+	for (int i = 2; i < n + 1; i++) {
+		float val = blockRandGauss.update();
+		float newMean = mean + (val - mean) / i;
+		sum += (val - mean) * (val - newMean);
+		mean = newMean;
+	}
+
+	float stdDev = sqrt(sum / (n - 1));
+	ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
+	ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace control
diff --git a/apps/systemlib/control/blocks.hpp b/apps/systemlib/control/blocks.hpp
new file mode 100644
index 000000000..95f2021ce
--- /dev/null
+++ b/apps/systemlib/control/blocks.hpp
@@ -0,0 +1,494 @@
+/****************************************************************************
+ *
+ *   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 blocks.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <assert.h>
+#include <time.h>
+#include <stdlib.h>
+#include <systemlib/test/test.hpp>
+
+#include "block/Block.hpp"
+#include "block/BlockParam.hpp"
+
+namespace control
+{
+
+int __EXPORT basicBlocksTest();
+
+/**
+ * A limiter/ saturation.
+ * The output of update is the input, bounded
+ * by min/max.
+ */
+class __EXPORT BlockLimit : public Block
+{
+public:
+// methods
+	BlockLimit(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_min(this, "MIN"),
+		_max(this, "MAX")
+	{};
+	virtual ~BlockLimit() {};
+	float update(float input);
+// accessors
+	float getMin() { return _min.get(); }
+	float getMax() { return _max.get(); }
+protected:
+// attributes
+	BlockParam<float> _min;
+	BlockParam<float> _max;
+};
+
+int __EXPORT blockLimitTest();
+
+/**
+ * A symmetric limiter/ saturation.
+ * Same as limiter but with only a max, is used for
+ * upper limit of +max, and lower limit of -max
+ */
+class __EXPORT BlockLimitSym : public Block
+{
+public:
+// methods
+	BlockLimitSym(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_max(this, "MAX")
+	{};
+	virtual ~BlockLimitSym() {};
+	float update(float input);
+// accessors
+	float getMax() { return _max.get(); }
+protected:
+// attributes
+	BlockParam<float> _max;
+};
+
+int __EXPORT blockLimitSymTest();
+
+/**
+ * A low pass filter as described here:
+ * http://en.wikipedia.org/wiki/Low-pass_filter.
+ */
+class __EXPORT BlockLowPass : public Block
+{
+public:
+// methods
+	BlockLowPass(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_state(0),
+		_fCut(this, "") // only one parameter, no need to name
+	{};
+	virtual ~BlockLowPass() {};
+	float update(float input);
+// accessors
+	float getState() { return _state; }
+	float getFCut() { return _fCut.get(); }
+	void setState(float state) { _state = state; }
+protected:
+// attributes
+	float _state;
+	BlockParam<float> _fCut;
+};
+
+int __EXPORT blockLowPassTest();
+
+/**
+ * A high pass filter as described here:
+ * http://en.wikipedia.org/wiki/High-pass_filter.
+ */
+class __EXPORT BlockHighPass : public Block
+{
+public:
+// methods
+	BlockHighPass(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_u(0),
+		_y(0),
+		_fCut(this, "") // only one parameter, no need to name
+	{};
+	virtual ~BlockHighPass() {};
+	float update(float input);
+// accessors
+	float getU() {return _u;}
+	float getY() {return _y;}
+	float getFCut() {return _fCut.get();}
+	void setU(float u) {_u = u;}
+	void setY(float y) {_y = y;}
+protected:
+// attributes
+	float _u; /**< previous input */
+	float _y; /**< previous output */
+	BlockParam<float> _fCut; /**< cut-off frequency, Hz */
+};
+
+int __EXPORT blockHighPassTest();
+
+/**
+ * A rectangular integrator.
+ * A limiter is built into the class to bound the
+ * integral's internal state. This is important
+ * for windup protection.
+ * @see Limit
+ */
+class __EXPORT BlockIntegral: public SuperBlock
+{
+public:
+// methods
+	BlockIntegral(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_y(0),
+		_limit(this, "") {};
+	virtual ~BlockIntegral() {};
+	float update(float input);
+// accessors
+	float getY() {return _y;}
+	float getMax() {return _limit.getMax();}
+	void setY(float y) {_y = y;}
+protected:
+// attributes
+	float _y; /**< previous output */
+	BlockLimitSym _limit; /**< limiter */
+};
+
+int __EXPORT blockIntegralTest();
+
+/**
+ * A trapezoidal integrator.
+ * http://en.wikipedia.org/wiki/Trapezoidal_rule
+ * A limiter is built into the class to bound the
+ * integral's internal state. This is important
+ * for windup protection.
+ * @see Limit
+ */
+class __EXPORT BlockIntegralTrap : public SuperBlock
+{
+public:
+// methods
+	BlockIntegralTrap(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_u(0),
+		_y(0),
+		_limit(this, "") {};
+	virtual ~BlockIntegralTrap() {};
+	float update(float input);
+// accessors
+	float getU() {return _u;}
+	float getY() {return _y;}
+	float getMax() {return _limit.getMax();}
+	void setU(float u) {_u = u;}
+	void setY(float y) {_y = y;}
+protected:
+// attributes
+	float _u; /**< previous input */
+	float _y; /**< previous output */
+	BlockLimitSym _limit; /**< limiter */
+};
+
+int __EXPORT blockIntegralTrapTest();
+
+/**
+ * A simple derivative approximation.
+ * This uses the previous and current input.
+ * This has a built in low pass filter.
+ * @see LowPass
+ */
+class __EXPORT BlockDerivative : public SuperBlock
+{
+public:
+// methods
+	BlockDerivative(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_u(0),
+		_lowPass(this, "LP")
+	{};
+	virtual ~BlockDerivative() {};
+	float update(float input);
+// accessors
+	void setU(float u) {_u = u;}
+	float getU() {return _u;}
+	float getLP() {return _lowPass.getFCut();}
+protected:
+// attributes
+	float _u; /**< previous input */
+	BlockLowPass _lowPass; /**< low pass filter */
+};
+
+int __EXPORT blockDerivativeTest();
+
+/**
+ * A proportional controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockP: public Block
+{
+public:
+// methods
+	BlockP(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_kP(this, "") // only one param, no need to name
+	{};
+	virtual ~BlockP() {};
+	float update(float input) {
+		return getKP() * input;
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+protected:
+	BlockParam<float> _kP;
+};
+
+int __EXPORT blockPTest();
+
+/**
+ * A proportional-integral controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockPI: public SuperBlock
+{
+public:
+// methods
+	BlockPI(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_integral(this, "I"),
+		_kP(this, "P"),
+		_kI(this, "I")
+	{};
+	virtual ~BlockPI() {};
+	float update(float input) {
+		return getKP() * input +
+		       getKI() * getIntegral().update(input);
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+	float getKI() { return _kI.get(); }
+	BlockIntegral &getIntegral() { return _integral; }
+private:
+	BlockIntegral _integral;
+	BlockParam<float> _kP;
+	BlockParam<float> _kI;
+};
+
+int __EXPORT blockPITest();
+
+/**
+ * A proportional-derivative controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockPD: public SuperBlock
+{
+public:
+// methods
+	BlockPD(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_derivative(this, "D"),
+		_kP(this, "P"),
+		_kD(this, "D")
+	{};
+	virtual ~BlockPD() {};
+	float update(float input) {
+		return getKP() * input +
+		       getKD() * getDerivative().update(input);
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+	float getKD() { return _kD.get(); }
+	BlockDerivative &getDerivative() { return _derivative; }
+private:
+	BlockDerivative _derivative;
+	BlockParam<float> _kP;
+	BlockParam<float> _kD;
+};
+
+int __EXPORT blockPDTest();
+
+/**
+ * A proportional-integral-derivative controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockPID: public SuperBlock
+{
+public:
+// methods
+	BlockPID(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_integral(this, "I"),
+		_derivative(this, "D"),
+		_kP(this, "P"),
+		_kI(this, "I"),
+		_kD(this, "D")
+	{};
+	virtual ~BlockPID() {};
+	float update(float input) {
+		return getKP() * input +
+		       getKI() * getIntegral().update(input) +
+		       getKD() * getDerivative().update(input);
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+	float getKI() { return _kI.get(); }
+	float getKD() { return _kD.get(); }
+	BlockIntegral &getIntegral() { return _integral; }
+	BlockDerivative &getDerivative() { return _derivative; }
+private:
+// attributes
+	BlockIntegral _integral;
+	BlockDerivative _derivative;
+	BlockParam<float> _kP;
+	BlockParam<float> _kI;
+	BlockParam<float> _kD;
+};
+
+int __EXPORT blockPIDTest();
+
+/**
+ * An output trim/ saturation block
+ */
+class __EXPORT BlockOutput: public SuperBlock
+{
+public:
+// methods
+	BlockOutput(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_trim(this, "TRIM"),
+		_limit(this, ""),
+		_val(0) {
+		update(0);
+	};
+	virtual ~BlockOutput() {};
+	void update(float input) {
+		_val = _limit.update(input + getTrim());
+	}
+// accessors
+	float getMin() { return _limit.getMin(); }
+	float getMax() { return _limit.getMax(); }
+	float getTrim() { return _trim.get(); }
+	float get() { return _val; }
+private:
+// attributes
+	BlockParam<float> _trim;
+	BlockLimit _limit;
+	float _val;
+};
+
+int __EXPORT blockOutputTest();
+
+/**
+ * A uniform random number generator
+ */
+class __EXPORT BlockRandUniform: public Block
+{
+public:
+// methods
+	BlockRandUniform(SuperBlock *parent,
+			 const char *name) :
+		Block(parent, name),
+		_min(this, "MIN"),
+		_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() {
+		static float rand_max = MAX_RAND;
+		float rand_val = rand();
+		float bounds = getMax() - getMin();
+		return getMin() + (rand_val * bounds) / rand_max;
+	}
+// accessors
+	float getMin() { return _min.get(); }
+	float getMax() { return _max.get(); }
+private:
+// attributes
+	BlockParam<float> _min;
+	BlockParam<float> _max;
+};
+
+int __EXPORT blockRandUniformTest();
+
+class __EXPORT BlockRandGauss: public Block
+{
+public:
+// methods
+	BlockRandGauss(SuperBlock *parent,
+		       const char *name) :
+		Block(parent, name),
+		_mean(this, "MEAN"),
+		_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() {
+		static float V1, V2, S;
+		static int phase = 0;
+		float X;
+
+		if (phase == 0) {
+			do {
+				float U1 = (float)rand() / MAX_RAND;
+				float U2 = (float)rand() / MAX_RAND;
+				V1 = 2 * U1 - 1;
+				V2 = 2 * U2 - 1;
+				S = V1 * V1 + V2 * V2;
+			} while (S >= 1 || fabsf(S) < 1e-8f);
+
+			X = V1 * float(sqrt(-2 * float(log(S)) / S));
+
+		} else
+			X = V2 * float(sqrt(-2 * float(log(S)) / S));
+
+		phase = 1 - phase;
+		return X * getStdDev() + getMean();
+	}
+// accessors
+	float getMean() { return _mean.get(); }
+	float getStdDev() { return _stdDev.get(); }
+private:
+// attributes
+	BlockParam<float> _mean;
+	BlockParam<float> _stdDev;
+};
+
+int __EXPORT blockRandGaussTest();
+
+} // namespace control
diff --git a/apps/systemlib/control/fixedwing.cpp b/apps/systemlib/control/fixedwing.cpp
new file mode 100644
index 000000000..695653714
--- /dev/null
+++ b/apps/systemlib/control/fixedwing.cpp
@@ -0,0 +1,351 @@
+/****************************************************************************
+ *
+ *   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 fixedwing.cpp
+ *
+ * Controller library code
+ */
+
+#include "fixedwing.hpp"
+
+namespace control
+{
+
+namespace fixedwing
+{
+
+BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_rLowPass(this, "R_LP"),
+	_rWashout(this, "R_HP"),
+	_r2Rdr(this, "R2RDR"),
+	_rudder(0)
+{
+}
+
+BlockYawDamper::~BlockYawDamper() {};
+
+void BlockYawDamper::update(float rCmd, float r)
+{
+	_rudder = _r2Rdr.update(rCmd -
+				_rWashout.update(_rLowPass.update(r)));
+}
+
+BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_yawDamper(this, ""),
+	_pLowPass(this, "P_LP"),
+	_qLowPass(this, "Q_LP"),
+	_p2Ail(this, "P2AIL"),
+	_q2Elv(this, "Q2ELV"),
+	_aileron(0),
+	_elevator(0)
+{
+}
+
+BlockStabilization::~BlockStabilization() {};
+
+void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
+				float p, float q, float r)
+{
+	_aileron = _p2Ail.update(
+			   pCmd - _pLowPass.update(p));
+	_elevator = _q2Elv.update(
+			    qCmd - _qLowPass.update(q));
+	_yawDamper.update(rCmd, r);
+}
+
+BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_psi2Phi(this, "PSI2PHI"),
+	_phi2P(this, "PHI2P"),
+	_phiLimit(this, "PHI_LIM")
+{
+}
+
+BlockHeadingHold::~BlockHeadingHold() {};
+
+float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
+{
+	float psiError = _wrap_pi(psiCmd - psi);
+	float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
+	return _phi2P.update(phiCmd - phi);
+}
+
+BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_v2Theta(this, "V2THE"),
+	_theta2Q(this, "THE2Q"),
+	_theLimit(this, "THE"),
+	_vLimit(this, "V")
+{
+}
+
+BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
+
+float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
+{
+	// negative sign because nose over to increase speed
+	float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
+	return _theta2Q.update(thetaCmd - theta);
+}
+
+BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_v2Thr(this, "V2THR")
+{
+}
+
+BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
+
+float BlockVelocityHoldFrontside::update(float vCmd, float v)
+{
+	return _v2Thr.update(vCmd - v);
+}
+
+BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_h2Thr(this, "H2THR"),
+	_throttle(0)
+{
+}
+
+BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
+
+void BlockAltitudeHoldBackside::update(float hCmd, float h)
+{
+	_throttle = _h2Thr.update(hCmd - h);
+}
+
+BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_h2Theta(this, "H2THE"),
+	_theta2Q(this, "THE2Q")
+{
+}
+
+BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
+
+float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
+{
+	float thetaCmd = _h2Theta.update(hCmd - h);
+	return _theta2Q.update(thetaCmd - theta);
+}
+
+BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
+		const char *name,
+		BlockStabilization *stabilization) :
+	SuperBlock(parent, name),
+	_stabilization(stabilization),
+	_headingHold(this, ""),
+	_velocityHold(this, ""),
+	_altitudeHold(this, ""),
+	_trimAil(this, "TRIM_AIL"),
+	_trimElv(this, "TRIM_ELV"),
+	_trimRdr(this, "TRIM_RDR"),
+	_trimThr(this, "TRIM_THR")
+{
+}
+
+BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
+
+void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
+				    float h, float v,
+				    float phi, float theta, float psi,
+				    float p, float q, float r)
+{
+	_altitudeHold.update(hCmd, h);
+	_stabilization->update(
+		_headingHold.update(psiCmd, phi, psi, p),
+		_velocityHold.update(vCmd, v, theta, q),
+		rCmd,
+		p, q, r);
+}
+
+BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_xtYawLimit(this, "XT2YAW"),
+	_xt2Yaw(this, "XT2YAW"),
+	_psiCmd(0)
+{
+}
+
+BlockWaypointGuidance::~BlockWaypointGuidance() {};
+
+void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
+				   vehicle_attitude_s &att,
+				   vehicle_global_position_setpoint_s &posCmd,
+				   vehicle_global_position_setpoint_s &lastPosCmd)
+{
+
+	// heading to waypoint
+	float psiTrack = get_bearing_to_next_waypoint(
+				 (double)pos.lat / (double)1e7d,
+				 (double)pos.lon / (double)1e7d,
+				 (double)posCmd.lat / (double)1e7d,
+				 (double)posCmd.lon / (double)1e7d);
+
+	// cross track
+	struct crosstrack_error_s xtrackError;
+	get_distance_to_line(&xtrackError,
+			     (double)pos.lat / (double)1e7d,
+			     (double)pos.lon / (double)1e7d,
+			     (double)lastPosCmd.lat / (double)1e7d,
+			     (double)lastPosCmd.lon / (double)1e7d,
+			     (double)posCmd.lat / (double)1e7d,
+			     (double)posCmd.lon / (double)1e7d);
+
+	_psiCmd = _wrap_2pi(psiTrack -
+			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
+}
+
+BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	// subscriptions
+	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
+	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
+	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
+	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
+	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
+	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
+	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
+	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+	// publications
+	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
+{
+}
+
+BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
+
+BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
+	BlockUorbEnabledAutopilot(parent, name),
+	_stabilization(this, ""), // no name needed, already unique
+	_backsideAutopilot(this, "", &_stabilization),
+	_guide(this, ""),
+	_vCmd(this, "V_CMD"),
+	_attPoll(),
+	_lastPosCmd(),
+	_timeStamp(0)
+{
+	_attPoll.fd = _att.getHandle();
+	_attPoll.events = POLLIN;
+}
+
+void BlockMultiModeBacksideAutopilot::update()
+{
+	// wait for a sensor update, check for exit condition every 100 ms
+	if (poll(&_attPoll, 1, 100) < 0) return; // poll error
+
+	uint64_t newTimeStamp = hrt_absolute_time();
+	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
+	_timeStamp = newTimeStamp;
+
+	// check for sane values of dt
+	// to prevent large control responses
+	if (dt > 1.0f || dt < 0) return;
+
+	// set dt for all child blocks
+	setDt(dt);
+
+	// store old position command before update if new command sent
+	if (_posCmd.updated()) {
+		_lastPosCmd = _posCmd.getData();
+	}
+
+	// check for new updates
+	if (_param_update.updated()) updateParams();
+
+	// get new information from subscriptions
+	updateSubscriptions();
+
+	// default all output to zero unless handled by mode
+	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
+		_actuators.control[i] = 0.0f;
+
+	// handle autopilot modes
+	if (_status.state_machine == SYSTEM_STATE_STABILIZED) {
+		_stabilization.update(
+			_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
+			_att.rollspeed, _att.pitchspeed, _att.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;
+
+	} else if (_status.state_machine == SYSTEM_STATE_AUTO) {
+		// update guidance
+		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+
+		// calculate velocity, XXX should be airspeed, but using ground speed for now
+		float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
+
+		// commands
+		float rCmd = 0;
+
+		_backsideAutopilot.update(
+			_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
+			_pos.alt, v,
+			_att.roll, _att.pitch, _att.yaw,
+			_att.rollspeed, _att.pitchspeed, _att.yawspeed
+		);
+		_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
+		_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
+		_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
+		_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
+
+	} else if (_status.state_machine == SYSTEM_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;
+	}
+
+	// update all publications
+	updatePublications();
+}
+
+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;
+
+	updatePublications();
+}
+
+} // namespace fixedwing
+
+} // namespace control
+
diff --git a/apps/systemlib/control/fixedwing.hpp b/apps/systemlib/control/fixedwing.hpp
new file mode 100644
index 000000000..ea742641c
--- /dev/null
+++ b/apps/systemlib/control/fixedwing.hpp
@@ -0,0 +1,438 @@
+/****************************************************************************
+ *
+ *   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 fixedwing.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <drivers/drv_hrt.h>
+#include <poll.h>
+
+#include "blocks.hpp"
+#include "block/UOrbSubscription.hpp"
+#include "block/UOrbPublication.hpp"
+
+extern "C" {
+#include <systemlib/geo/geo.h>
+}
+
+namespace control
+{
+
+namespace fixedwing
+{
+
+/**
+ * BlockYawDamper
+ *
+ * This block has more explations to help new developers
+ * add their own blocks. It includes a limited explanation
+ * of some C++ basics.
+ *
+ * Block: The generic class describing a typical block as you
+ * would expect in Simulink or ScicosLab. A block can have
+ * parameters. It cannot have other blocks.
+ *
+ * SuperBlock: A superblock is a block that can have other
+ * blocks. It has methods that manage the blocks beneath it.
+ *
+ * BlockYawDamper inherits from SuperBlock publically, this
+ * means that any public function in SuperBlock are public within
+ * BlockYawDamper and may be called from outside the
+ * class methods. Any protected function within block
+ * are private to the class and may not be called from
+ * outside this class. Protected should be preferred
+ * where possible to public as it is important to
+ * limit access to the bare minimum to prevent
+ * accidental errors.
+ */
+class __EXPORT BlockYawDamper : public SuperBlock
+{
+private:
+	/**
+	 * Declaring other blocks used by this block
+	 *
+	 * In this section we declare all child blocks that
+	 * this block is composed of. They are private
+	 * members so only this block has direct access to
+	 * them.
+	 */
+	BlockLowPass _rLowPass;
+	BlockHighPass _rWashout;
+	BlockP _r2Rdr;
+
+	/**
+	 * Declaring output values and accessors
+	 *
+	 * If we have any output values for the block we
+	 * declare them here. Output can be directly returned
+	 * through the update function, but outputs should be
+	 * declared here if the information will likely be requested
+	 * again, or if there are multiple outputs.
+	 *
+	 * You should only be able to set outputs from this block,
+	 * so the outputs are declared in the private section.
+	 * A get accessor is provided
+	 * in the public section for other blocks to get the
+	 * value of the output.
+	 */
+	float _rudder;
+public:
+	/**
+	 * BlockYawDamper Constructor
+	 *
+	 * The job of the constructor is to initialize all
+	 * parameter in this block and initialize all child
+	 * blocks. Note also, that the output values must be
+	 * initialized here. The order of the members in the
+	 * member initialization list should follow the
+	 * order in which they are declared within the class.
+	 * See the private member declarations above.
+	 *
+	 * Block Construction
+	 *
+	 * All blocks are constructed with their parent block
+	 * and their name. This allows parameters within the
+	 * block to construct a fully qualified name from
+	 * concatenating the two. If the name provided to the
+	 * block is "", then the block will use the parent
+	 * name as it's name. This is useful in cases where
+	 * you have a block that has parameters "MIN", "MAX",
+	 * such as BlockLimit and you do not want an extra name
+	 * to qualify them since the parent block has no "MIN",
+	 * "MAX" parameters.
+	 *
+	 * Block Parameter Construction
+	 *
+	 * Block parameters are named constants, they are
+	 * constructed using:
+	 * BlockParam::BlockParam(Block * parent, const char * name)
+	 * This funciton takes both a parent block and a name.
+	 * The constructore then uses the parent name and the name of
+	 * the paramter to ask the px4 param library if it has any
+	 * parameters with this name. If it does, a handle to the
+	 * parameter is retrieved.
+	 *
+	 * Block/ BlockParam Naming
+	 *
+	 * When desigining new blocks, the naming of the parameters and
+	 * blocks determines the fully qualified name of the parameters
+	 * within the ground station, so it is important to choose
+	 * short, easily understandable names. Again, when a name of
+	 * "" is passed, the parent block name is used as the value to
+	 * prepend to paramters names.
+	 */
+	BlockYawDamper(SuperBlock *parent, const char *name);
+	/**
+	 * Block deconstructor
+	 *
+	 * It is always a good idea to declare a virtual
+	 * deconstructor so that upon calling delete from
+	 * a class derived from this, all of the
+	 * deconstructors all called, the derived class first, and
+	 * then the base class
+	 */
+	virtual ~BlockYawDamper();
+
+	/**
+	 * Block update function
+	 *
+	 * The job of the update function is to compute the output
+	 * values for the block. In a simple block with one output,
+	 * the output may be returned directly. If the output is
+	 * required frequenly by other processses, it might be a
+	 * good idea to declare a member to store the temporary
+	 * variable.
+	 */
+	void update(float rCmd, float r);
+
+	/**
+	 * Rudder output value accessor
+	 *
+	 * This is a public accessor function, which means that the
+	 * private value _rudder is returned to anyone calling
+	 * BlockYawDamper::getRudder(). Note thate a setRudder() is
+	 * not provided, this is because the updateParams() call
+	 * for a block provides the mechanism for updating the
+	 * paramter.
+	 */
+	float getRudder() { return _rudder; }
+};
+
+/**
+ * Stability augmentation system.
+ * Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299
+ */
+class __EXPORT BlockStabilization : public SuperBlock
+{
+private:
+	BlockYawDamper _yawDamper;
+	BlockLowPass _pLowPass;
+	BlockLowPass _qLowPass;
+	BlockP _p2Ail;
+	BlockP _q2Elv;
+	float _aileron;
+	float _elevator;
+public:
+	BlockStabilization(SuperBlock *parent, const char *name);
+	virtual ~BlockStabilization();
+	void update(float pCmd, float qCmd, float rCmd,
+		    float p, float q, float r);
+	float getAileron() { return _aileron; }
+	float getElevator() { return _elevator; }
+	float getRudder() { return _yawDamper.getRudder(); }
+};
+
+/**
+ * Heading hold autopilot block.
+ * Aircraft Control and Simulation, Stevens and Lewis
+ * Heading hold, pg. 348
+ */
+class __EXPORT BlockHeadingHold : public SuperBlock
+{
+private:
+	BlockP _psi2Phi;
+	BlockP _phi2P;
+	BlockLimitSym _phiLimit;
+public:
+	BlockHeadingHold(SuperBlock *parent, const char *name);
+	virtual ~BlockHeadingHold();
+	/**
+	 * returns pCmd
+	 */
+	float update(float psiCmd, float phi, float psi, float p);
+};
+
+/**
+ * Frontside/ Backside Control Systems
+ *
+ * Frontside :
+ *   velocity error -> throttle
+ *   altitude error -> elevator
+ *
+ * Backside :
+ *   velocity error -> elevator
+ *   altitude error -> throttle
+ *
+ * Backside control systems are more resilient at
+ * slow speeds on the back-side of the power
+ * required curve/ landing etc. Less performance
+ * than frontside at high speeds.
+ */
+
+/**
+ * Backside velocity hold autopilot block.
+ * v -> theta -> q -> elevator
+ */
+class __EXPORT BlockVelocityHoldBackside : public SuperBlock
+{
+private:
+	BlockPID _v2Theta;
+	BlockPID _theta2Q;
+	BlockLimit _theLimit;
+	BlockLimit _vLimit;
+public:
+	BlockVelocityHoldBackside(SuperBlock *parent, const char *name);
+	virtual ~BlockVelocityHoldBackside();
+	/**
+	 * returns qCmd
+	 */
+	float update(float vCmd, float v, float theta, float q);
+};
+
+/**
+ * Frontside velocity hold autopilot block.
+ * v -> throttle
+ */
+class __EXPORT BlockVelocityHoldFrontside : public SuperBlock
+{
+private:
+	BlockPID _v2Thr;
+public:
+	BlockVelocityHoldFrontside(SuperBlock *parent, const char *name);
+	virtual ~BlockVelocityHoldFrontside();
+	/**
+	 * returns throttle
+	 */
+	float update(float vCmd, float v);
+};
+
+/**
+ * Backside altitude hold autopilot block.
+ * h -> throttle
+ */
+class __EXPORT BlockAltitudeHoldBackside : public SuperBlock
+{
+private:
+	BlockPID _h2Thr;
+	float _throttle;
+public:
+	BlockAltitudeHoldBackside(SuperBlock *parent, const char *name);
+	virtual ~BlockAltitudeHoldBackside();
+	void update(float hCmd, float h);
+	float getThrottle() { return _throttle; }
+};
+
+/**
+ * Frontside altitude hold autopilot block.
+ * h -> theta > q -> elevator
+ */
+class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock
+{
+private:
+	BlockPID _h2Theta;
+	BlockPID _theta2Q;
+public:
+	BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name);
+	virtual ~BlockAltitudeHoldFrontside();
+	/**
+	 * return qCmd
+	 */
+	float update(float hCmd, float h, float theta, float q);
+};
+
+/**
+ * Backside autopilot
+ */
+class __EXPORT BlockBacksideAutopilot : public SuperBlock
+{
+private:
+	BlockStabilization *_stabilization;
+	BlockHeadingHold _headingHold;
+	BlockVelocityHoldBackside _velocityHold;
+	BlockAltitudeHoldBackside _altitudeHold;
+	BlockParam<float> _trimAil;
+	BlockParam<float> _trimElv;
+	BlockParam<float> _trimRdr;
+	BlockParam<float> _trimThr;
+public:
+	BlockBacksideAutopilot(SuperBlock *parent,
+			       const char *name,
+			       BlockStabilization *stabilization);
+	virtual ~BlockBacksideAutopilot();
+	void update(float hCmd, float vCmd, float rCmd, float psiCmd,
+		    float h, float v,
+		    float phi, float theta, float psi,
+		    float p, float q, float r);
+	float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); }
+	float getAileron() { return _stabilization->getAileron() + _trimAil.get(); }
+	float getElevator() { return _stabilization->getElevator() + _trimElv.get(); }
+	float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); }
+};
+
+/**
+ * Waypoint Guidance block
+ */
+class __EXPORT BlockWaypointGuidance : public SuperBlock
+{
+private:
+	BlockLimitSym _xtYawLimit;
+	BlockP _xt2Yaw;
+	float _psiCmd;
+public:
+	BlockWaypointGuidance(SuperBlock *parent, const char *name);
+	virtual ~BlockWaypointGuidance();
+	void update(vehicle_global_position_s &pos,
+		    vehicle_attitude_s &att,
+		    vehicle_global_position_setpoint_s &posCmd,
+		    vehicle_global_position_setpoint_s &lastPosCmd);
+	float getPsiCmd() { return _psiCmd; }
+};
+
+/**
+ * UorbEnabledAutopilot
+ */
+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<vehicle_global_position_setpoint_s> _posCmd;
+	UOrbSubscription<manual_control_setpoint_s> _manual;
+	UOrbSubscription<vehicle_status_s> _status;
+	UOrbSubscription<parameter_update_s> _param_update;
+	// publications
+	UOrbPublication<actuator_controls_s> _actuators;
+public:
+	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
+	virtual ~BlockUorbEnabledAutopilot();
+};
+
+/**
+ * Multi-mode Autopilot
+ */
+class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
+{
+private:
+	BlockStabilization _stabilization;
+	BlockBacksideAutopilot _backsideAutopilot;
+	BlockWaypointGuidance _guide;
+	BlockParam<float> _vCmd;
+
+	struct pollfd _attPoll;
+	vehicle_global_position_setpoint_s _lastPosCmd;
+	enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
+	uint64_t _timeStamp;
+public:
+	BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name);
+	void update();
+	virtual ~BlockMultiModeBacksideAutopilot();
+};
+
+
+} // namespace fixedwing
+
+} // namespace control
+
diff --git a/apps/systemlib/control/test_params.c b/apps/systemlib/control/test_params.c
new file mode 100644
index 000000000..7c609cad3
--- /dev/null
+++ b/apps/systemlib/control/test_params.c
@@ -0,0 +1,22 @@
+#include <systemlib/param/param.h>
+// WARNING:
+//  do not changes these unless
+//  you want to recompute the
+//  answers for all of the unit tests
+
+PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f);
+PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f);
+PARAM_DEFINE_FLOAT(TEST_HP, 10.0f);
+PARAM_DEFINE_FLOAT(TEST_LP, 10.0f);
+
+PARAM_DEFINE_FLOAT(TEST_P, 0.2f);
+
+PARAM_DEFINE_FLOAT(TEST_I, 0.1f);
+PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f);
+
+PARAM_DEFINE_FLOAT(TEST_D, 0.01f);
+PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f);
+
+PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f);
+PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f);