Merge remote-tracking branch 'upstream/ROS_shared_lib_base_class' into dev_ros_rossharedlib

Conflicts:
	src/modules/uORB/topics/vehicle_attitude.h
	src/platforms/px4_defines.h

.catkin_workspace

@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration

src/lib/mathlib/math/Limits.cpp

@@ -46,6 +46,9 @@
 
 namespace math {
 
+#ifndef CONFIG_ARCH_ARM	
+#define M_PI_F 3.14159265358979323846f
+#endif
 
 float __EXPORT min(float val1, float val2)
 {

src/lib/mathlib/math/Limits.hpp

@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include <nuttx/config.h>
+#include <platforms/px4_defines.h>
 #include <stdint.h>
 
 namespace math {

src/lib/mathlib/math/Matrix.hpp

@@ -44,12 +44,20 @@
 #define MATRIX_HPP
 
 #include <stdio.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <math/eigen_math.h>
+#include <Eigen/Eigen>
+#define M_PI_2_F 1.5707963267948966192f
+#endif
 
 namespace math
 {
 
-template <unsigned int M, unsigned int N>
+template<unsigned int M, unsigned int N>
 class __EXPORT Matrix;
 
 // MxN matrix with float elements
@@ -65,16 +73,19 @@ public:
 	/**
 	 * struct for using arm_math functions
 	 */
+#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_mat;
+#else
+	eigen_matrix_instance arm_mat;
+#endif
 
 	/**
 	 * trivial ctor
 	 * Initializes the elements to zero.
 	 */
 	MatrixBase() :
-		data{},
+		data {},
-		arm_mat{M, N, &data[0][0]}
+	arm_mat {M, N, &data[0][0]} {
-	{
 	}
 
 	virtual ~MatrixBase() {};
@@ -83,20 +94,17 @@ public:
 	 * copyt ctor
 	 */
 	MatrixBase(const MatrixBase<M, N> &m) :
-		arm_mat{M, N, &data[0][0]}
+		arm_mat {M, N, &data[0][0]} {
-	{
 		memcpy(data, m.data, sizeof(data));
 	}
 
 	MatrixBase(const float *d) :
-		arm_mat{M, N, &data[0][0]}
+		arm_mat {M, N, &data[0][0]} {
-	{
 		memcpy(data, d, sizeof(data));
 	}
 
 	MatrixBase(const float d[M][N]) :
-		arm_mat{M, N, &data[0][0]}
+		arm_mat {M, N, &data[0][0]} {
-	{
 		memcpy(data, d, sizeof(data));
 	}
 
@@ -148,8 +156,9 @@ public:
 	bool operator ==(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
 			for (unsigned int j = 0; j < N; j++)
-				if (data[i][j] != m.data[i][j])
+				if (data[i][j] != m.data[i][j]) {
 					return false;
+				}
 
 		return true;
 	}
@@ -160,8 +169,9 @@ public:
 	bool operator !=(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
 			for (unsigned int j = 0; j < N; j++)
-				if (data[i][j] != m.data[i][j])
+				if (data[i][j] != m.data[i][j]) {
 					return true;
+				}
 
 		return false;
 	}
@@ -181,8 +191,9 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
+			for (unsigned int j = 0; j < M; j++) {
 				res.data[i][j] = -data[i][j];
+			}
 
 		return res;
 	}
@@ -194,16 +205,18 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
+			for (unsigned int j = 0; j < M; j++) {
 				res.data[i][j] = data[i][j] + m.data[i][j];
+			}
 
 		return res;
 	}
 
 	Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
+			for (unsigned int j = 0; j < M; j++) {
 				data[i][j] += m.data[i][j];
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -215,16 +228,18 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				res.data[i][j] = data[i][j] - m.data[i][j];
+			}
 
 		return res;
 	}
 
 	Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
+			for (unsigned int j = 0; j < M; j++) {
 				data[i][j] -= m.data[i][j];
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -236,16 +251,19 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				res.data[i][j] = data[i][j] * num;
+			}
 
 		return res;
+
 	}
 
 	Matrix<M, N> &operator *=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				data[i][j] *= num;
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -254,16 +272,18 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				res[i][j] = data[i][j] / num;
+			}
 
 		return res;
 	}
 
 	Matrix<M, N> &operator /=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				data[i][j] /= num;
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -273,27 +293,53 @@ public:
 	 */
 	template <unsigned int P>
 	Matrix<M, P> operator *(const Matrix<N, P> &m) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<M, P> res;
 		arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::Matrix<float, N, P, Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float, N, P, Eigen::RowMajor> >
+				(m.arm_mat.pData);
+		Eigen::Matrix<float, M, P, Eigen::RowMajor> Product = Me * Him;
+		Matrix<M, P> res(Product.data());
+		return res;
+#endif
 	}
 
 	/**
 	 * transpose the matrix
 	 */
 	Matrix<N, M> transposed(void) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<N, M> res;
 		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Me.transposeInPlace();
+		Matrix<N, M> res(Me.data());
+		return res;
+#endif
 	}
 
 	/**
 	 * invert the matrix
 	 */
 	Matrix<M, N> inversed(void) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<M, N> res;
 		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
+		Matrix<M, N> res(MyInverse.data());
+		return res;
+#endif
 	}
 
 	/**
@@ -310,16 +356,18 @@ public:
 		memset(data, 0, sizeof(data));
 		unsigned int n = (M < N) ? M : N;
 
-		for (unsigned int i = 0; i < n; i++)
+		for (unsigned int i = 0; i < n; i++) {
 			data[i][i] = 1;
 		}
+	}
 
 	void print(void) {
 		for (unsigned int i = 0; i < M; i++) {
 			printf("[ ");
 
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				printf("%.3f\t", data[i][j]);
+			}
 
 			printf(" ]\n");
 		}
@@ -352,8 +400,17 @@ public:
 	 * multiplication by a vector
 	 */
 	Vector<M> operator *(const Vector<N> &v) const {
+#ifdef CONFIG_ARCH_ARM
 		Vector<M> res;
 		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
+#else
+		//probably nicer if this could go into a function like "eigen_mat_mult" or so
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData, N);
+		Eigen::VectorXf Product = Me * Vec;
+		Vector<M> res(Product.data());
+#endif
 		return res;
 	}
 };

src/lib/mathlib/math/Quaternion.hpp

@@ -44,7 +44,7 @@
 #define QUATERNION_HPP
 
 #include <math.h>
-#include "../CMSIS/Include/arm_math.h"
+
 #include "Vector.hpp"
 #include "Matrix.hpp"
 

src/lib/mathlib/math/Vector.hpp

@@ -45,7 +45,12 @@
 
 #include <stdio.h>
 #include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <math/eigen_math.h>
+#endif
 
 namespace math
 {
@@ -65,7 +70,12 @@ public:
 	/**
 	 * struct for using arm_math functions, represents column vector
 	 */
+	#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_col;
+	#else
+	eigen_matrix_instance arm_col;
+	#endif
+
 
 	/**
 	 * trivial ctor

src/modules/fw_att_control/fw_att_control_base.cpp

@@ -0,0 +1,326 @@
+/* Copyright (c) 2014 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 mc_att_control_base.cpp
+ * 
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include "fw_att_control_base.h"
+#include <math.h>
+#include <mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
+#include <systemlib/err.h>
+
+using namespace std;
+
+FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() :
+
+		_task_should_exit(false), _task_running(false), _control_task(-1),
+
+		/* performance counters */
+		_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(
+				perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(
+				perf_alloc(PC_COUNT, "fw att control nonfinite output")),
+		/* states */
+		_setpoint_valid(false), _debug(false) {
+	/* safely initialize structs */
+	_att = {};
+	_att_sp = {};
+	_manual = {};
+	_airspeed = {};
+	_vcontrol_mode = {};
+	_actuators = {};
+	_actuators_airframe = {};
+	_global_pos = {};
+	_vehicle_status = {};
+
+}
+
+FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() {
+
+}
+
+void FixedwingAttitudeControlBase::control_attitude() {
+	bool lock_integrator = false;
+	static int loop_counter = 0;
+	/* scale around tuning airspeed */
+
+	float airspeed;
+
+	/* if airspeed is not updating, we assume the normal average speed */
+	if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s)
+			|| hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
+		airspeed = _parameters.airspeed_trim;
+		if (nonfinite) {
+			perf_count(_nonfinite_input_perf);
+		}
+	} else {
+		/* prevent numerical drama by requiring 0.5 m/s minimal speed */
+		airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
+	}
+
+	/*
+	 * For scaling our actuators using anything less than the min (close to stall)
+	 * speed doesn't make any sense - its the strongest reasonable deflection we
+	 * want to do in flight and its the baseline a human pilot would choose.
+	 *
+	 * Forcing the scaling to this value allows reasonable handheld tests.
+	 */
+
+	float airspeed_scaling = _parameters.airspeed_trim
+			/ ((airspeed < _parameters.airspeed_min) ?
+					_parameters.airspeed_min : airspeed);
+
+	float roll_sp = _parameters.rollsp_offset_rad;
+	float pitch_sp = _parameters.pitchsp_offset_rad;
+	float throttle_sp = 0.0f;
+
+	if (_vcontrol_mode.flag_control_velocity_enabled
+			|| _vcontrol_mode.flag_control_position_enabled) {
+		/* read in attitude setpoint from attitude setpoint uorb topic */
+		roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
+		pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
+		throttle_sp = _att_sp.thrust;
+
+		/* reset integrals where needed */
+		if (_att_sp.roll_reset_integral) {
+			_roll_ctrl.reset_integrator();
+		}
+		if (_att_sp.pitch_reset_integral) {
+			_pitch_ctrl.reset_integrator();
+		}
+		if (_att_sp.yaw_reset_integral) {
+			_yaw_ctrl.reset_integrator();
+		}
+	} else {
+		/*
+		 * Scale down roll and pitch as the setpoints are radians
+		 * and a typical remote can only do around 45 degrees, the mapping is
+		 * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
+		 *
+		 * With this mapping the stick angle is a 1:1 representation of
+		 * the commanded attitude.
+		 *
+		 * The trim gets subtracted here from the manual setpoint to get
+		 * the intended attitude setpoint. Later, after the rate control step the
+		 * trim is added again to the control signal.
+		 */
+		roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+				+ _parameters.rollsp_offset_rad;
+		pitch_sp = -(_manual.x * _parameters.man_pitch_max
+				- _parameters.trim_pitch) + _parameters.pitchsp_offset_rad;
+		throttle_sp = _manual.z;
+		_actuators.control[4] = _manual.flaps;
+
+		/*
+		 * in manual mode no external source should / does emit attitude setpoints.
+		 * emit the manual setpoint here to allow attitude controller tuning
+		 * in attitude control mode.
+		 */
+		struct vehicle_attitude_setpoint_s att_sp;
+		att_sp.timestamp = hrt_absolute_time();
+		att_sp.roll_body = roll_sp;
+		att_sp.pitch_body = pitch_sp;
+		att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
+		att_sp.thrust = throttle_sp;
+
+	}
+
+	/* If the aircraft is on ground reset the integrators */
+	if (_vehicle_status.condition_landed) {
+		_roll_ctrl.reset_integrator();
+		_pitch_ctrl.reset_integrator();
+		_yaw_ctrl.reset_integrator();
+	}
+
+	/* Prepare speed_body_u and speed_body_w */
+	float speed_body_u = 0.0f;
+	float speed_body_v = 0.0f;
+	float speed_body_w = 0.0f;
+	if (_att.R_valid) {
+		speed_body_u = _att.R[0][0] * _global_pos.vel_n
+				+ _att.R[1][0] * _global_pos.vel_e
+				+ _att.R[2][0] * _global_pos.vel_d;
+		speed_body_v = _att.R[0][1] * _global_pos.vel_n
+				+ _att.R[1][1] * _global_pos.vel_e
+				+ _att.R[2][1] * _global_pos.vel_d;
+		speed_body_w = _att.R[0][2] * _global_pos.vel_n
+				+ _att.R[1][2] * _global_pos.vel_e
+				+ _att.R[2][2] * _global_pos.vel_d;
+	} else {
+		if (_debug && loop_counter % 10 == 0) {
+			warnx("Did not get a valid R\n");
+		}
+	}
+
+	/* Run attitude controllers */
+	if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+		_roll_ctrl.control_attitude(roll_sp, _att.roll);
+		_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
+		_yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u,
+				speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(),
+				_pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+
+		/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
+		float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed,
+				_att.yawspeed, _yaw_ctrl.get_desired_rate(),
+				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
+				airspeed_scaling, lock_integrator);
+		_actuators.control[0] =
+				(isfinite(roll_u)) ?
+						roll_u + _parameters.trim_roll : _parameters.trim_roll;
+		if (!isfinite(roll_u)) {
+			_roll_ctrl.reset_integrator();
+			perf_count(_nonfinite_output_perf);
+
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("roll_u %.4f", (double) roll_u);
+			}
+		}
+
+		float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
+				_att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(),
+				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
+				airspeed_scaling, lock_integrator);
+		_actuators.control[1] =
+				(isfinite(pitch_u)) ?
+						pitch_u + _parameters.trim_pitch :
+						_parameters.trim_pitch;
+		if (!isfinite(pitch_u)) {
+			_pitch_ctrl.reset_integrator();
+			perf_count(_nonfinite_output_perf);
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
+						" airspeed %.4f, airspeed_scaling %.4f,"
+						" roll_sp %.4f, pitch_sp %.4f,"
+						" _roll_ctrl.get_desired_rate() %.4f,"
+						" _pitch_ctrl.get_desired_rate() %.4f"
+						" att_sp.roll_body %.4f", (double) pitch_u,
+						(double) _yaw_ctrl.get_desired_rate(),
+						(double) airspeed, (double) airspeed_scaling,
+						(double) roll_sp, (double) pitch_sp,
+						(double) _roll_ctrl.get_desired_rate(),
+						(double) _pitch_ctrl.get_desired_rate(),
+						(double) _att_sp.roll_body);
+			}
+		}
+
+		float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
+				_att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(),
+				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
+				airspeed_scaling, lock_integrator);
+		_actuators.control[2] =
+				(isfinite(yaw_u)) ?
+						yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
+		if (!isfinite(yaw_u)) {
+			_yaw_ctrl.reset_integrator();
+			perf_count(_nonfinite_output_perf);
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("yaw_u %.4f", (double) yaw_u);
+			}
+		}
+
+		/* throttle passed through */
+		_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+		if (!isfinite(throttle_sp)) {
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("throttle_sp %.4f", (double) throttle_sp);
+			}
+		}
+	} else {
+		perf_count(_nonfinite_input_perf);
+		if (_debug && loop_counter % 10 == 0) {
+			warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f",
+					(double) roll_sp, (double) pitch_sp);
+		}
+	}
+
+}
+
+void FixedwingAttitudeControlBase::set_attitude(const Eigen::Quaternion<double> attitude) {
+	// watch out, still need to see where we modify attitude for the tailsitter case
+	math::Quaternion quat;
+	quat(0) = (float)attitude.w();
+	quat(1) = (float)attitude.x();
+	quat(2) = (float)attitude.y();
+	quat(3) = (float)attitude.z();
+
+	_att.q[0] = quat(0);
+	_att.q[1] = quat(1);
+	_att.q[2] = quat(2);
+	_att.q[3] = quat(3);
+
+	math::Matrix<3,3> Rot = quat.to_dcm();
+	_att.R[0][0] = Rot(0,0);
+	_att.R[1][0] = Rot(1,0);
+	_att.R[2][0] = Rot(2,0);
+	_att.R[0][1] = Rot(0,1);
+	_att.R[1][1] = Rot(1,1);
+	_att.R[2][1] = Rot(2,1);
+	_att.R[0][2] = Rot(0,2);
+	_att.R[1][2] = Rot(1,2);
+	_att.R[2][2] = Rot(2,2);
+
+	_att.R_valid = true;
+}
+void FixedwingAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
+	_att.rollspeed  = angular_rate(0);
+	_att.pitchspeed = angular_rate(1);
+	_att.yawspeed   = angular_rate(2);
+}
+void FixedwingAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
+	_att_sp.roll_body  = control_attitude_thrust_reference(0);
+	_att_sp.pitch_body = control_attitude_thrust_reference(1);
+	_att_sp.yaw_body   = control_attitude_thrust_reference(2);
+	_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
+
+	// setup rotation matrix
+	math::Matrix<3,3> Rot_sp;
+	Rot_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
+	_att_sp.R_body[0][0] = Rot_sp(0,0);
+	_att_sp.R_body[1][0] = Rot_sp(1,0);
+	_att_sp.R_body[2][0] = Rot_sp(2,0);
+	_att_sp.R_body[0][1] = Rot_sp(0,1);
+	_att_sp.R_body[1][1] = Rot_sp(1,1);
+	_att_sp.R_body[2][1] = Rot_sp(2,1);
+	_att_sp.R_body[0][2] = Rot_sp(0,2);
+	_att_sp.R_body[1][2] = Rot_sp(1,2);
+	_att_sp.R_body[2][2] = Rot_sp(2,2);
+}
+void FixedwingAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) {
+	motor_inputs(0) = _actuators.control[0];
+	motor_inputs(1) = _actuators.control[1];
+	motor_inputs(2) = _actuators.control[2];
+	motor_inputs(3) = _actuators.control[3];
+}

src/modules/fw_att_control/fw_att_control_base.h

@@ -0,0 +1,151 @@
+#ifndef FW_ATT_CONTROL_BASE_H_
+#define FW_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 2014 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 fw_att_control_base.h
+ * 
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include <ecl/attitude_fw/ecl_pitch_controller.h>
+#include <ecl/attitude_fw/ecl_roll_controller.h>
+#include <ecl/attitude_fw/ecl_yaw_controller.h>
+
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include <systemlib/perf_counter.h>
+
+class FixedwingAttitudeControlBase
+{
+public:
+	/**
+	 * Constructor
+	 */
+	FixedwingAttitudeControlBase();
+
+	/**
+	 * Destructor
+	 */
+	~FixedwingAttitudeControlBase();
+
+
+protected:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		_task_running;			/**< if true, task is running in its mainloop */
+	int		_control_task;			/**< task handle for sensor task */
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
+	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
+	struct airspeed_s				_airspeed;		/**< airspeed */
+	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
+	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
+	struct actuator_controls_s			_actuators_airframe;	/**< actuator control inputs */
+	struct vehicle_global_position_s		_global_pos;		/**< global position */
+	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
+	perf_counter_t	_nonfinite_output_perf;		/**< performance counter for non finite output */
+
+	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
+	bool		_debug;				/**< if set to true, print debug output */
+
+	struct {
+		float tconst;
+		float p_p;
+		float p_d;
+		float p_i;
+		float p_ff;
+		float p_rmax_pos;
+		float p_rmax_neg;
+		float p_integrator_max;
+		float p_roll_feedforward;
+		float r_p;
+		float r_d;
+		float r_i;
+		float r_ff;
+		float r_integrator_max;
+		float r_rmax;
+		float y_p;
+		float y_i;
+		float y_d;
+		float y_ff;
+		float y_roll_feedforward;
+		float y_integrator_max;
+		float y_coordinated_min_speed;
+		float y_rmax;
+
+		float airspeed_min;
+		float airspeed_trim;
+		float airspeed_max;
+
+		float trim_roll;
+		float trim_pitch;
+		float trim_yaw;
+		float rollsp_offset_deg;			/**< Roll Setpoint Offset in deg */
+		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
+		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
+		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
+		float man_roll_max;						/**< Max Roll in rad */
+		float man_pitch_max;					/**< Max Pitch in rad */
+
+	}		_parameters;			/**< local copies of interesting parameters */
+
+	ECL_RollController				_roll_ctrl;
+	ECL_PitchController				_pitch_ctrl;
+	ECL_YawController				_yaw_ctrl;
+
+	void control_attitude();
+
+	// setters and getters for interface with euroc-gazebo simulator
+	void set_attitude(const Eigen::Quaternion<double> attitude);
+	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
+	void get_mixer_input(Eigen::Vector4d& motor_inputs);
+	
+};
+
+#endif /* FW_ATT_CONTROL_BASE_H_ */

src/modules/mc_att_control/mc_att_control_base.cpp

@@ -0,0 +1,403 @@
+/* Copyright (c) 2014 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 mc_att_control_base.h
+ * 
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include "mc_att_control_base.h"
+#include <geo/geo.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
+#else
+#include <cmath>
+using namespace std;
+#endif
+
+MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
+
+		_task_should_exit(false), _control_task(-1),
+
+		_actuators_0_circuit_breaker_enabled(false),
+
+		/* performance counters */
+		_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+
+{
+	memset(&_v_att, 0, sizeof(_v_att));
+	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
+	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_actuators, 0, sizeof(_actuators));
+	memset(&_armed, 0, sizeof(_armed));
+
+	_params.att_p.zero();
+	_params.rate_p.zero();
+	_params.rate_i.zero();
+	_params.rate_d.zero();
+	_params.yaw_ff = 0.0f;
+	_params.yaw_rate_max = 0.0f;
+	_params.man_roll_max = 0.0f;
+	_params.man_pitch_max = 0.0f;
+	_params.man_yaw_max = 0.0f;
+	_params.acro_rate_max.zero();
+
+	_rates_prev.zero();
+	_rates_sp.zero();
+	_rates_int.zero();
+	_thrust_sp = 0.0f;
+	_att_control.zero();
+
+	_I.identity();
+
+	// setup standard gains
+	_params.att_p(0)      = 5.0;
+	_params.rate_p(0)     = 0.05;
+	_params.rate_i(0)     = 0.0;
+	_params.rate_d(0)     = 0.003;
+	/* pitch gains */
+	_params.att_p(1)      = 5.0;
+	_params.rate_p(1)     = 0.05;
+	_params.rate_i(1)     = 0.0;
+	_params.rate_d(1)     = 0.003;
+	/* yaw gains */
+	_params.att_p(2)      = 2.8;
+	_params.rate_p(2)     = 0.2;
+	_params.rate_i(2)     = 0.1;
+	_params.rate_d(2)     = 0.0;
+	_params.yaw_rate_max  = 0.5;
+	_params.yaw_ff        = 0.5;
+	_params.man_roll_max  = 0.6;
+	_params.man_pitch_max = 0.6;
+	_params.man_yaw_max   = 0.6;
+}
+
+MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() {
+}
+
+void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() {
+}
+
+void MulticopterAttitudeControlBase::control_attitude(float dt) {
+	float yaw_sp_move_rate = 0.0f;
+	bool publish_att_sp = false;
+
+	if (_v_control_mode.flag_control_manual_enabled) {
+		/* manual input, set or modify attitude setpoint */
+
+		if (_v_control_mode.flag_control_velocity_enabled
+				|| _v_control_mode.flag_control_climb_rate_enabled) {
+			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
+			vehicle_attitude_setpoint_poll();
+		}
+
+		if (!_v_control_mode.flag_control_climb_rate_enabled) {
+			/* pass throttle directly if not in altitude stabilized mode */
+			_v_att_sp.thrust = _manual_control_sp.z;
+			publish_att_sp = true;
+		}
+
+		if (!_armed.armed) {
+			/* reset yaw setpoint when disarmed */
+			_reset_yaw_sp = true;
+		}
+
+		/* move yaw setpoint in all modes */
+		if (_v_att_sp.thrust < 0.1f) {
+			// TODO
+			//if (_status.condition_landed) {
+			/* reset yaw setpoint if on ground */
+			//	reset_yaw_sp = true;
+			//}
+		} else {
+			/* move yaw setpoint */
+			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
+			_v_att_sp.yaw_body = _wrap_pi(
+					_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
+			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+			if (yaw_offs < -yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
+
+			} else if (yaw_offs > yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
+			}
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+		/* reset yaw setpint to current position if needed */
+		if (_reset_yaw_sp) {
+			_reset_yaw_sp = false;
+			_v_att_sp.yaw_body = _v_att.yaw;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+		if (!_v_control_mode.flag_control_velocity_enabled) {
+			/* update attitude setpoint if not in position control mode */
+			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
+			_v_att_sp.pitch_body = -_manual_control_sp.x
+					* _params.man_pitch_max;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+	} else {
+		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
+		vehicle_attitude_setpoint_poll();
+
+		/* reset yaw setpoint after non-manual control mode */
+		_reset_yaw_sp = true;
+	}
+
+	_thrust_sp = _v_att_sp.thrust;
+
+	/* construct attitude setpoint rotation matrix */
+	math::Matrix<3, 3> R_sp;
+
+	if (_v_att_sp.R_valid) {
+		/* rotation matrix in _att_sp is valid, use it */
+		R_sp.set(&_v_att_sp.R_body[0][0]);
+
+	} else {
+		/* rotation matrix in _att_sp is not valid, use euler angles instead */
+		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body,
+				_v_att_sp.yaw_body);
+
+		/* copy rotation matrix back to setpoint struct */
+		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0],
+				sizeof(_v_att_sp.R_body));
+		_v_att_sp.R_valid = true;
+	}
+
+//	/* publish the attitude setpoint if needed */
+//	if (publish_att_sp) {
+//		_v_att_sp.timestamp = hrt_absolute_time();
+//
+//		if (_att_sp_pub > 0) {
+//			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
+//					&_v_att_sp);
+//
+//		} else {
+//			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
+//					&_v_att_sp);
+//		}
+//	}
+
+	/* rotation matrix for current state */
+	math::Matrix<3, 3> R;
+	R.set(_v_att.R);
+
+	/* all input data is ready, run controller itself */
+
+	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
+	math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2));
+	math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+
+	/* axis and sin(angle) of desired rotation */
+	math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);
+
+	/* calculate angle error */
+	float e_R_z_sin = e_R.length();
+	float e_R_z_cos = R_z * R_sp_z;
+
+	/* calculate weight for yaw control */
+	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
+
+	/* calculate rotation matrix after roll/pitch only rotation */
+	math::Matrix<3, 3> R_rp;
+
+	if (e_R_z_sin > 0.0f) {
+		/* get axis-angle representation */
+		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
+		math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin;
+
+		e_R = e_R_z_axis * e_R_z_angle;
+
+		/* cross product matrix for e_R_axis */
+		math::Matrix<3, 3> e_R_cp;
+		e_R_cp.zero();
+		e_R_cp(0, 1) = -e_R_z_axis(2);
+		e_R_cp(0, 2) = e_R_z_axis(1);
+		e_R_cp(1, 0) = e_R_z_axis(2);
+		e_R_cp(1, 2) = -e_R_z_axis(0);
+		e_R_cp(2, 0) = -e_R_z_axis(1);
+		e_R_cp(2, 1) = e_R_z_axis(0);
+
+		/* rotation matrix for roll/pitch only rotation */
+		R_rp = R
+				* (_I + e_R_cp * e_R_z_sin
+						+ e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+
+	} else {
+		/* zero roll/pitch rotation */
+		R_rp = R;
+	}
+
+	/* R_rp and R_sp has the same Z axis, calculate yaw error */
+	math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
+	math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
+
+	if (e_R_z_cos < 0.0f) {
+		/* for large thrust vector rotations use another rotation method:
+		 * calculate angle and axis for R -> R_sp rotation directly */
+		math::Quaternion q;
+		q.from_dcm(R.transposed() * R_sp);
+		math::Vector < 3 > e_R_d = q.imag();
+		e_R_d.normalize();
+		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
+
+		/* use fusion of Z axis based rotation and direct rotation */
+		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
+		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
+	}
+
+	/* calculate angular rates setpoint */
+	_rates_sp = _params.att_p.emult(e_R);
+
+	/* limit yaw rate */
+	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max,
+			_params.yaw_rate_max);
+
+	/* feed forward yaw setpoint rate */
+	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+}
+
+void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
+	/* reset integral if disarmed */
+	if (!_armed.armed) {
+		_rates_int.zero();
+	}
+
+	/* current body angular rates */
+	math::Vector < 3 > rates;
+	rates(0) = _v_att.rollspeed;
+	rates(1) = _v_att.pitchspeed;
+	rates(2) = _v_att.yawspeed;
+
+	/* angular rates error */
+	math::Vector < 3 > rates_err = _rates_sp - rates;
+	_att_control = _params.rate_p.emult(rates_err)
+			+ _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
+	_rates_prev = rates;
+
+	/* update integral only if not saturated on low limit */
+	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
+		for (int i = 0; i < 3; i++) {
+			if (fabsf(_att_control(i)) < _thrust_sp) {
+				float rate_i = _rates_int(i)
+						+ _params.rate_i(i) * rates_err(i) * dt;
+
+				if (isfinite(
+						rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+						_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+					_rates_int(i) = rate_i;
+				}
+			}
+		}
+	}
+
+}
+
+void MulticopterAttitudeControlBase::set_actuator_controls() {
+	_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+	_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+	_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+	_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+}
+
+void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion<double> attitude) {
+	math::Quaternion quat;
+	quat(0) = (float)attitude.w();
+	quat(1) = (float)attitude.x();
+	quat(2) = (float)attitude.y();
+	quat(3) = (float)attitude.z();
+
+	_v_att.q[0] = quat(0);
+	_v_att.q[1] = quat(1);
+	_v_att.q[2] = quat(2);
+	_v_att.q[3] = quat(3);
+
+	math::Matrix<3,3> Rot = quat.to_dcm();
+	_v_att.R[0][0] = Rot(0,0);
+	_v_att.R[1][0] = Rot(1,0);
+	_v_att.R[2][0] = Rot(2,0);
+	_v_att.R[0][1] = Rot(0,1);
+	_v_att.R[1][1] = Rot(1,1);
+	_v_att.R[2][1] = Rot(2,1);
+	_v_att.R[0][2] = Rot(0,2);
+	_v_att.R[1][2] = Rot(1,2);
+	_v_att.R[2][2] = Rot(2,2);
+
+	_v_att.R_valid = true;
+}
+
+void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
+	// check if this is consistent !!!
+	_v_att.rollspeed  = angular_rate(0);
+	_v_att.pitchspeed = angular_rate(1);
+	_v_att.yawspeed   = angular_rate(2);
+}
+
+void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
+	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
+	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
+	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
+	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
+
+	// setup rotation matrix
+	math::Matrix<3,3> Rot_sp;
+	Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body);
+	_v_att_sp.R_body[0][0] = Rot_sp(0,0);
+	_v_att_sp.R_body[1][0] = Rot_sp(1,0);
+	_v_att_sp.R_body[2][0] = Rot_sp(2,0);
+	_v_att_sp.R_body[0][1] = Rot_sp(0,1);
+	_v_att_sp.R_body[1][1] = Rot_sp(1,1);
+	_v_att_sp.R_body[2][1] = Rot_sp(2,1);
+	_v_att_sp.R_body[0][2] = Rot_sp(0,2);
+	_v_att_sp.R_body[1][2] = Rot_sp(1,2);
+	_v_att_sp.R_body[2][2] = Rot_sp(2,2);
+}
+
+void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) {
+	motor_inputs(0) = _actuators.control[0];
+	motor_inputs(1) = _actuators.control[1];
+	motor_inputs(2) = _actuators.control[2];
+	motor_inputs(3) = _actuators.control[3];
+}
+

src/modules/mc_att_control/mc_att_control_base.h

@@ -0,0 +1,138 @@
+#ifndef MC_ATT_CONTROL_BASE_H_
+#define MC_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 2014 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 mc_att_control_base.h
+ * 
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <lib/mathlib/mathlib.h>
+
+
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+class MulticopterAttitudeControlBase {
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControlBase();
+
+	/**
+	 * Destructor
+	 */
+	~MulticopterAttitudeControlBase();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return		OK on success.
+	 */
+	void control_attitude(float dt);
+	void control_attitude_rates(float dt);
+	
+	 // setters and getters for interface with euroc-gazebo simulator
+	void set_attitude(const Eigen::Quaternion<double> attitude);
+	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
+	void get_mixer_input(Eigen::Vector4d& motor_inputs);
+	void set_actuator_controls();
+
+protected:
+
+	bool _task_should_exit; /**< if true, sensor task should exit */
+	int _control_task; /**< task handle for sensor task */
+	bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
+
+	struct vehicle_attitude_s _v_att; /**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
+	struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
+	struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
+	struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
+	struct actuator_controls_s _actuators; /**< actuator controls */
+	struct actuator_armed_s _armed; /**< actuator arming status */
+
+	perf_counter_t _loop_perf; /**< loop performance counter */
+
+	math::Vector<3> _rates_prev; /**< angular rates on previous step */
+	math::Vector<3> _rates_sp; /**< angular rates setpoint */
+	math::Vector<3> _rates_int; /**< angular rates integral error */
+	float _thrust_sp; /**< thrust setpoint */
+	math::Vector<3> _att_control; /**< attitude control vector */
+
+	math::Matrix<3, 3> _I; /**< identity matrix */
+
+	bool _reset_yaw_sp; /**< reset yaw setpoint flag */
+
+	struct {
+		math::Vector<3> att_p; /**< P gain for angular error */
+		math::Vector<3> rate_p; /**< P gain for angular rate error */
+		math::Vector<3> rate_i; /**< I gain for angular rate error */
+		math::Vector<3> rate_d; /**< D gain for angular rate error */
+		float yaw_ff; /**< yaw control feed-forward */
+		float yaw_rate_max; /**< max yaw rate */
+
+		float man_roll_max;
+		float man_pitch_max;
+		float man_yaw_max;
+		math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
+	} _params;
+	
+	void vehicle_attitude_setpoint_poll();	//provisional
+
+};
+
+#endif /* MC_ATT_CONTROL_BASE_H_ */

src/modules/uORB/topics/actuator_armed.h

@@ -42,7 +42,7 @@
 #define TOPIC_ACTUATOR_ARMED_H
 
 #include <stdint.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics

src/modules/uORB/topics/actuator_controls.h

@@ -47,7 +47,7 @@
 #define TOPIC_ACTUATOR_CONTROLS_H
 
 #include <stdint.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 #define NUM_ACTUATOR_CONTROLS		8
 #define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */

src/modules/uORB/topics/airspeed.h

@@ -40,7 +40,7 @@
 #ifndef TOPIC_AIRSPEED_H_
 #define TOPIC_AIRSPEED_H_
 
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 #include <stdint.h>
 
 /**

src/modules/uORB/topics/fence.h

@@ -42,7 +42,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics

src/modules/uORB/topics/manual_control_setpoint.h

@@ -41,7 +41,7 @@
 #define TOPIC_MANUAL_CONTROL_SETPOINT_H_
 
 #include <stdint.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * Switch position

src/modules/uORB/topics/parameter_update.h

@@ -40,7 +40,7 @@
 #define TOPIC_PARAMETER_UPDATE_H
 
 #include <stdint.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics

src/modules/uORB/topics/vehicle_attitude_setpoint.h

@@ -42,7 +42,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics

src/modules/uORB/topics/vehicle_control_mode.h

@@ -48,7 +48,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 #include "vehicle_status.h"
 
 /**

src/modules/uORB/topics/vehicle_global_position.h

@@ -45,7 +45,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics

src/modules/uORB/topics/vehicle_rates_setpoint.h

@@ -41,7 +41,7 @@
 #define TOPIC_VEHICLE_RATES_SETPOINT_H_
 
 #include <stdint.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics

src/modules/uORB/topics/vehicle_status.h

@@ -53,7 +53,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics @{

src/platforms/ros/perf_counter.cpp

@@ -0,0 +1,142 @@
+/*
+ * perf_counter.c
+
+ *
+ *  Created on: Sep 24, 2014
+ *      Author: roman
+ */
+#include <stdlib.h>
+#include <stdio.h>
+#include <systemlib/perf_counter.h>
+
+
+
+perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name)
+{
+	return NULL;
+}
+
+/**
+ * Free a counter.
+ *
+ * @param handle		The performance counter's handle.
+ */
+void perf_free(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Count a performance event.
+ *
+ * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_count(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Begin a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_begin(perf_counter_t handle)
+{
+
+}
+
+/**
+ * End a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_end(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_cancel(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Reset a performance counter.
+ *
+ * This call resets performance counter to initial state
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_reset(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print one performance counter to stdout
+ *
+ * @param handle		The counter to print.
+ */
+void		perf_print_counter(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print one performance counter to a fd.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ * @param handle		The counter to print.
+ */
+void		perf_print_counter_fd(int fd, perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print all of the performance counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ */
+void		perf_print_all(int fd)
+{
+
+}
+
+/**
+ * Reset all of the performance counters.
+ */
+void		perf_reset_all(void)
+{
+
+}
+
+/**
+ * Return current event_count
+ *
+ * @param handle		The counter returned from perf_alloc.
+ * @return			event_count
+ */
+uint64_t	perf_event_count(perf_counter_t handle)
+{
+
+}
+
+