Merge James's controllib bits into a separate library module.

Add a top-level mathlib header to avoid having to dig around for specific headers.

14 files changed, 0 insertions, 2710 deletions

diff --git a/apps/systemlib/control/block/Block.cpp b/apps/systemlib/control/block/Block.cpp
deleted file mode 100644
index 5994d2315..000000000
--- a/apps/systemlib/control/block/Block.cpp
+++ /dev/null
@@ -1,210 +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 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
deleted file mode 100644
index 258701f27..000000000
--- a/apps/systemlib/control/block/Block.hpp
+++ /dev/null
@@ -1,131 +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 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
deleted file mode 100644
index b3f49f7db..000000000
--- a/apps/systemlib/control/block/BlockParam.cpp
+++ /dev/null
@@ -1,77 +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 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
deleted file mode 100644
index 7f86d1717..000000000
--- a/apps/systemlib/control/block/BlockParam.hpp
+++ /dev/null
@@ -1,85 +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 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
deleted file mode 100644
index 96b0b94d1..000000000
--- a/apps/systemlib/control/block/List.hpp
+++ /dev/null
@@ -1,71 +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 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
deleted file mode 100644
index f69b39d90..000000000
--- a/apps/systemlib/control/block/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/apps/systemlib/control/block/UOrbPublication.hpp b/apps/systemlib/control/block/UOrbPublication.hpp
deleted file mode 100644
index a36f4429f..000000000
--- a/apps/systemlib/control/block/UOrbPublication.hpp
+++ /dev/null
@@ -1,118 +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.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
deleted file mode 100644
index 022cadd24..000000000
--- a/apps/systemlib/control/block/UOrbSubscription.cpp
+++ /dev/null
@@ -1,51 +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 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
deleted file mode 100644
index 22cc2e114..000000000
--- a/apps/systemlib/control/block/UOrbSubscription.hpp
+++ /dev/null
@@ -1,137 +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 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
deleted file mode 100644
index c6c374300..000000000
--- a/apps/systemlib/control/blocks.cpp
+++ /dev/null
@@ -1,486 +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 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
deleted file mode 100644
index 95f2021ce..000000000
--- a/apps/systemlib/control/blocks.hpp
+++ /dev/null
@@ -1,494 +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 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
deleted file mode 100644
index 695653714..000000000
--- a/apps/systemlib/control/fixedwing.cpp
+++ /dev/null
@@ -1,351 +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 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
deleted file mode 100644
index ea742641c..000000000
--- a/apps/systemlib/control/fixedwing.hpp
+++ /dev/null
@@ -1,438 +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 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
deleted file mode 100644
index 7c609cad3..000000000
--- a/apps/systemlib/control/test_params.c
+++ /dev/null
@@ -1,22 +0,0 @@
-#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);