Working on segway controller, restructure of fixedwing.

11 files changed, 165 insertions, 733 deletions

diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp
index 5994d2315..b964d40a3 100644
--- a/src/modules/controllib/block/Block.cpp
+++ b/src/modules/controllib/block/Block.cpp
@@ -43,8 +43,8 @@
 
 #include "Block.hpp"
 #include "BlockParam.hpp"
-#include "UOrbSubscription.hpp"
-#include "UOrbPublication.hpp"
+#include "../uorb/UOrbSubscription.hpp"
+#include "../uorb/UOrbPublication.hpp"
 
 namespace control
 {
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 7a785d12e..fefe99702 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -42,6 +42,7 @@
 #include <assert.h>
 #include <time.h>
 #include <stdlib.h>
+#include <math.h>
 #include <mathlib/math/test/test.hpp>
 
 #include "block/Block.hpp"
diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp
deleted file mode 100644
index 77b2ac806..000000000
--- a/src/modules/controllib/fixedwing.cpp
+++ /dev/null
@@ -1,379 +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, float outputScale)
-{
-	_rudder = outputScale*_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, float outputScale)
-{
-	_aileron = outputScale*_p2Ail.update(
-			   pCmd - _pLowPass.update(p));
-	_elevator = outputScale*_q2Elv.update(
-			    qCmd - _qLowPass.update(q));
-	_yawDamper.update(rCmd, r, outputScale);
-}
-
-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_set_triplet), 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
-
-	// heading hold
-	_psi2Phi(this, "PSI2PHI"),
-	_phi2P(this, "PHI2P"),
-	_phiLimit(this, "PHI_LIM"),
-
-	// velocity hold
-	_v2Theta(this, "V2THE"),
-	_theta2Q(this, "THE2Q"),
-	_theLimit(this, "THE"),
-	_vLimit(this, "V"),
-
-	// altitude/climb rate hold
-	_h2Thr(this, "H2THR"),
-	_cr2Thr(this, "CR2THR"),
-
-	// guidance block
-	_guide(this, ""),
-
-	_trimAil(this, "TRIM_ROLL", false), 	/* general roll trim (full name: TRIM_ROLL) */
-	_trimElv(this, "TRIM_PITCH", false), 	/* general pitch trim */
-	_trimRdr(this, "TRIM_YAW", false), 	/* general yaw trim */
-	_trimThr(this, "TRIM_THR"), 	/* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
-	_trimV(this, "TRIM_V"), 	/* FWB_ specific trim velocity (full name : FWB_TRIM_V) */
-
-	_vCmd(this, "V_CMD"),
-	_crMax(this, "CR_MAX"),
-	_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;
-
-	// only update guidance in auto mode
-	if (_status.state_machine == SYSTEM_STATE_AUTO) {
-		// update guidance
-		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
-	}
-
-	// XXX handle STABILIZED (loiter on spot) as well
-	// once the system switches from manual or auto to stabilized
-	// the setpoint should update to loitering around this position
-
-	// handle autopilot modes
-	if (_status.state_machine == SYSTEM_STATE_AUTO ||
-	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
-
-		// update guidance
-		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
-
-		// calculate velocity, XXX should be airspeed, but using ground speed for now
-		// for the purpose of control we will limit the velocity feedback between
-		// the min/max velocity
-		float v = _vLimit.update(sqrtf(
-					_pos.vx * _pos.vx +
-					_pos.vy * _pos.vy +
-					_pos.vz * _pos.vz));
-
-		// limit velocity command between min/max velocity
-		float vCmd = _vLimit.update(_vCmd.get());
-
-		// altitude hold
-		float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
-
-		// heading hold
-		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
-		float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
-		float pCmd = _phi2P.update(phiCmd - _att.roll);
-
-		// velocity hold
-		// negative sign because nose over to increase speed
-		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
-		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
-
-		// yaw rate cmd
-		float rCmd = 0;
-
-		// stabilization
-		float velocityRatio = _trimV.get()/v;
-		float outputScale = velocityRatio*velocityRatio;
-		// this term scales the output based on the dynamic pressure change from trim
-		_stabilization.update(pCmd, qCmd, rCmd,
-				      _att.rollspeed, _att.pitchspeed, _att.yawspeed,
-				      outputScale);
-
-		// output
-		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
-		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
-		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
-		_actuators.control[CH_THR] = dThrottle + _trimThr.get();
-
-		// XXX limit throttle to manual setting (safety) for now.
-		// If it turns out to be confusing, it can be removed later once
-		// a first binary release can be targeted.
-		// This is not a hack, but a design choice.
-
-		/* do not limit in HIL */
-		if (!_status.flag_hil_enabled) {
-			/* limit to value of manual throttle */
-			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-						     _actuators.control[CH_THR] : _manual.throttle;
-		}
-
-	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
-
-		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-			_actuators.control[CH_AIL] = _manual.roll;
-			_actuators.control[CH_ELV] = _manual.pitch;
-			_actuators.control[CH_RDR] = _manual.yaw;
-			_actuators.control[CH_THR] = _manual.throttle;
-
-		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-
-			// calculate velocity, XXX should be airspeed, but using ground speed for now
-			// for the purpose of control we will limit the velocity feedback between
-			// the min/max velocity
-			float v = _vLimit.update(sqrtf(
-						_pos.vx * _pos.vx +
-						_pos.vy * _pos.vy +
-						_pos.vz * _pos.vz));
-
-			// pitch channel -> rate of climb
-			// TODO, might want to put a gain on this, otherwise commanding
-			// from +1 -> -1 m/s for rate of climb
-			//float dThrottle = _cr2Thr.update(
-			//_crMax.get()*_manual.pitch - _pos.vz);
-
-			// roll channel -> bank angle
-			float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
-			float pCmd = _phi2P.update(phiCmd - _att.roll);
-
-			// throttle channel -> velocity
-			// negative sign because nose over to increase speed
-			float vCmd = _vLimit.update(_manual.throttle *
-					(_vLimit.getMax() - _vLimit.getMin()) +
-					_vLimit.getMin());
-			float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
-			float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
-
-			// yaw rate cmd
-			float rCmd = 0;
-
-			// stabilization
-			_stabilization.update(pCmd, qCmd, rCmd,
-					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
-
-			// output
-			_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
-			_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
-			_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
-
-			// currently using manual throttle
-			// XXX if you enable this watch out, vz might be very noisy
-			//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
-			_actuators.control[CH_THR] = _manual.throttle;
-
-			// XXX limit throttle to manual setting (safety) for now.
-			// If it turns out to be confusing, it can be removed later once
-			// a first binary release can be targeted.
-			// This is not a hack, but a design choice.
-
-			/* do not limit in HIL */
-			if (!_status.flag_hil_enabled) {
-				/* limit to value of manual throttle */
-				_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-							     _actuators.control[CH_THR] : _manual.throttle;
-			}
-		}
-
-		// body rates controller, disabled for now
-		else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
-
-			_stabilization.update(_manual.roll, _manual.pitch, _manual.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;
-		}
-	}
-
-	// 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/src/modules/controllib/fixedwing.hpp b/src/modules/controllib/fixedwing.hpp
deleted file mode 100644
index e4028c40d..000000000
--- a/src/modules/controllib/fixedwing.hpp
+++ /dev/null
@@ -1,344 +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_set_triplet.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, float outputScale = 1.0);
-
-	/**
-	 * 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 outputScale = 1.0);
-	float getAileron() { return _aileron; }
-	float getElevator() { return _elevator; }
-	float getRudder() { return _yawDamper.getRudder(); }
-};
-
-/**
- * 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.
- */
-
-/**
- * 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_set_triplet_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:
-	// stabilization
-	BlockStabilization _stabilization;
-
-	// heading hold
-	BlockP _psi2Phi;
-	BlockP _phi2P;
-	BlockLimitSym _phiLimit;
-
-	// velocity hold
-	BlockPID _v2Theta;
-	BlockPID _theta2Q;
-	BlockLimit _theLimit;
-	BlockLimit _vLimit;
-
-	// altitude/ climb rate hold
-	BlockPID _h2Thr;
-	BlockPID _cr2Thr;
-
-	// guidance
-	BlockWaypointGuidance _guide;
-
-	// block params
-	BlockParam<float> _trimAil;
-	BlockParam<float> _trimElv;
-	BlockParam<float> _trimRdr;
-	BlockParam<float> _trimThr;
-	BlockParam<float> _trimV;
-	BlockParam<float> _vCmd;
-	BlockParam<float> _crMax;
-
-	struct pollfd _attPoll;
-	vehicle_global_position_set_triplet_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/src/modules/controllib/module.mk b/src/modules/controllib/module.mk
index 13d1069c7..d815a8feb 100644
--- a/src/modules/controllib/module.mk
+++ b/src/modules/controllib/module.mk
@@ -37,7 +37,7 @@
 SRCS		 = test_params.c \
 		   block/Block.cpp \
 		   block/BlockParam.cpp \
-		   block/UOrbPublication.cpp \
-		   block/UOrbSubscription.cpp \
-		   blocks.cpp \
-		   fixedwing.cpp
+		   uorb/UOrbPublication.cpp \
+		   uorb/UOrbSubscription.cpp \
+		   uorb/blocks.cpp \
+		   blocks.cpp
diff --git a/src/modules/controllib/block/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp
index f69b39d90..f69b39d90 100644
--- a/src/modules/controllib/block/UOrbPublication.cpp
+++ b/src/modules/controllib/uorb/UOrbPublication.cpp
diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/uorb/UOrbPublication.hpp
index 0a8ae2ff7..6f1f3fc1c 100644
--- a/src/modules/controllib/block/UOrbPublication.hpp
+++ b/src/modules/controllib/uorb/UOrbPublication.hpp
@@ -39,8 +39,8 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include "Block.hpp"
-#include "List.hpp"
+#include "../block/Block.hpp"
+#include "../block/List.hpp"
 
 
 namespace control
diff --git a/src/modules/controllib/block/UOrbSubscription.cpp b/src/modules/controllib/uorb/UOrbSubscription.cpp
index 022cadd24..022cadd24 100644
--- a/src/modules/controllib/block/UOrbSubscription.cpp
+++ b/src/modules/controllib/uorb/UOrbSubscription.cpp
diff --git a/src/modules/controllib/block/UOrbSubscription.hpp b/src/modules/controllib/uorb/UOrbSubscription.hpp
index 22cc2e114..d337d89a8 100644
--- a/src/modules/controllib/block/UOrbSubscription.hpp
+++ b/src/modules/controllib/uorb/UOrbSubscription.hpp
@@ -39,8 +39,8 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include "Block.hpp"
-#include "List.hpp"
+#include "../block/Block.hpp"
+#include "../block/List.hpp"
 
 
 namespace control
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
new file mode 100644
index 000000000..6e5ade519
--- /dev/null
+++ b/src/modules/controllib/uorb/blocks.cpp
@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   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 uorb_blocks.cpp
+ *
+ * uorb block library code
+ */
+
+#include "blocks.hpp"
+
+namespace control
+{
+
+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_set_triplet), 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() {};
+
+} // namespace control
+
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
new file mode 100644
index 000000000..54f31735c
--- /dev/null
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   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 uorb_blocks.h
+ *
+ * uorb block 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_set_triplet.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 "UOrbSubscription.hpp"
+#include "UOrbPublication.hpp"
+
+namespace control
+{
+
+/**
+ * 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_set_triplet_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();
+};
+
+} // namespace control
+