mc_att_control_vector: initial rewrite, WIP

4 files changed, 66 insertions, 364 deletions

diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp
deleted file mode 100644
index 589c31c0d..000000000
--- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp
+++ /dev/null
@@ -1,147 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author:    Tobias Naegeli
- *
- * 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 ecl_mc_att_control_vector.cpp
- *
- * Multirotor attitude controller based on concepts in:
- *
- * Minimum Snap Trajectory Generation and Control for Quadrotors
- * http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf
- *
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- *
- */
-
-#include <mathlib/mathlib.h>
-#include "ecl_mc_att_control_vector.h"
-
-ECL_MCAttControlVector::ECL_MCAttControlVector() :
-    _integral_error(0.0f, 0.0f),
-    _integral_max(1000.0f, 1000.0f),
-    _integral_lock(false)
-    {
-
-    }
-
-void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des_in,
-                                float Kp, float Kd, float Ki, const math::Vector &angular_rates,
-                                math::Vector &rates_des, float &thrust)
-{
-    // XXX
-    bool earth = true;
-    bool integral_reset = false;
-     
-    math::Matrix R_bn = R_nb.transpose();
-     
-    float cy = cosf(yaw);
-    float sy = sinf(yaw);
-     
-    math::Matrix RYaw = math::Dcm(cy,-sy,0.0f,sy,cy,0.0f,0.0f,0.0f,1.0f);
-    math::Vector z_b = math::Vector3(R_bn(0,2), R_bn(1,2), R_bn(2,2));
-
-    math::Vector3 F_des = F_des_in;
-     
-    // desired thrust in body frame
-    // avoid division by zero
-    // compensates for thrust loss due to roll/pitch
-    //XXX disable this for first time
-    //if (F_des(2) >= 0.1f) {
-    //    thrust = F_des(2) / R_bn(2, 2);
-    //} else {
-    //    F_des(2) = 0.1f;
-    //    thrust= F_des(2) / R_bn(2, 2);
-    //}
-
-    math::Vector3 x_B_des;
-    math::Vector3 y_B_des;
-    math::Vector3 z_B_des;
-     
-    // desired body z axis
-    if (earth) {
-        z_B_des = (F_des / F_des.norm());
-    } else {
-        z_B_des = RYaw * (F_des / F_des.norm());
-    }
-
-    // desired direction in world coordinates (yaw angle)
-    math::Vector3 x_C(cy, sy, 0.0f);
-    // desired body y axis
-    y_B_des = z_B_des.cross(x_C) / (z_B_des.cross(x_C)).norm();
-    // desired body x axis
-    x_B_des = y_B_des.cross(z_B_des);
-    // desired Rotation Matrix
-    math::Matrix R_des = math::Dcm(x_B_des(0), x_B_des(1), x_B_des(2),
-                                   y_B_des(0), y_B_des(1), y_B_des(2),
-                                   z_B_des(0), z_B_des(1), z_B_des(2));
-     
-     
-    // Attitude Controller
-    // P controller
-     
-    // error rotation matrix
-    math::Matrix e_R = (R_des.transpose() * R_bn - R_bn.transpose() * R_des) * 0.5f;
-     
-    // error rotation vector
-    math::Vector e_R_v(3);
-    e_R_v(0) = e_R(1,2);
-    e_R_v(1) = e_R(0,2);
-    e_R_v(2) = e_R(0,1);
-     
-     
-    // include an integral part
-    math::Vector intError = math::Vector3(0.0f, 0.0f, 0.0f);
-    if (!_integral_lock) {
-        if (thrust > 0.3f && !integral_reset) {
-            if (fabsf(_integral_error(0)) < _integral_max(0)) {
-                _integral_error(0) = _integral_error(0) + e_R_v(0) * dt;
-            }
-
-            if (fabsf(_integral_error(1)) < _integral_max(1)) {
-                _integral_error(1) = _integral_error(1) + e_R_v(1) * dt;
-            }
-           
-        } else {
-            _integral_error(0) = 0.0f;
-            _integral_error(1) = 0.0f;
-        }
-        
-        intError(0) = _integral_error(0);
-        intError(1) = _integral_error(1);
-        intError(2) = 0.0f;
-    }
-
-    rates_des = (e_R_v * Kp + angular_rates * Kd + intError * Ki);
-}
diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h
deleted file mode 100644
index a3440a06b..000000000
--- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author:    Tobias Naegeli
- *
- * 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 ecl_mc_att_control_vector.cpp
- *
- * Multirotor attitude controller based on concepts in:
- *
- * Minimum Snap Trajectory Generation and Control for Quadrotors
- * http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf
- *
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- *
- */
-
-#include <mathlib/mathlib.h>
-
-class ECL_MCAttControlVector {
-
-public:
-	ECL_MCAttControlVector();
-	void control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des,
-                                float Kp, float Kd, float Ki, const math::Vector &angular_rates,
-                                math::Vector &rates_des, float &thrust);
-
-	void set_imax(float integral_max) {
-		_integral_max(0) = integral_max;
-		_integral_max(1) = integral_max;
-	}
-
-	void reset_integral() {
-		_integral_error(0) = 0.0f;
-		_integral_error(1) = 0.0f;
-	}
-
-	void lock_integrator(bool lock) {
-		_integral_lock = lock;
-	}
-
-protected:
-    math::Vector2f _integral_error;
-    math::Vector2f _integral_max;
-    bool _integral_lock;
-};
diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
index e0b36067c..1783efc0e 100644
--- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
@@ -1,7 +1,9 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Lorenz Meier
+ *   Author: 	Tobias Naegeli <naegelit@student.ethz.ch>
+ *   			Lorenz Meier <lm@inf.ethz.ch>
+ *   			Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,13 +36,16 @@
 
 /**
  * @file mc_att_control_vector_main.c
- * Implementation of a multicopter attitude controller based on desired thrust vector.
+ * Implementation of a multicopter attitude controller based on desired rotation matrix.
+ *
+ * References
+ *  [1] Daniel Mellinger and Vijay Kumar, "Minimum Snap Trajectory Generation and Control for Quadrotors",
+ *  	http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  *
- * Please refer to the library files for the authors and acknowledgements of
- * the used control library functions.
  */
 
 #include <nuttx/config.h>
@@ -54,7 +59,6 @@
 #include <poll.h>
 #include <time.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_accel.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -72,8 +76,6 @@
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
 
-#include "ecl_mc_att_control_vector.h"
-
 /**
  * Multicopter attitude control app start / stop handling function
  *
@@ -107,7 +109,6 @@ private:
 	int		_control_task;			/**< task handle for sensor task */
 
 	int		_att_sub;			/**< vehicle attitude subscription */
-	int		_accel_sub;			/**< accelerometer subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_control_mode_sub;			/**< vehicle control mode subscription */
@@ -119,52 +120,27 @@ private:
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct accel_report				_accel;			/**< body frame accelerations */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
 	struct vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
 	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
 	struct actuator_armed_s				_arming;		/**< actuator arming status */
+	struct vehicle_rates_setpoint_s		_rates_setpoint;		/**< vehicle rates setpoint */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
 
-	struct {
-		float yaw_p;
-		float yaw_i;
-		float yaw_d;
-		float yaw_imax;
-
-		float att_p;
-		float att_i;
-		float att_d;
-		float att_imax;
-
-		float att_rate_p;
-
-		float yaw_rate_p;
-	}		_parameters;			/**< local copies of interesting parameters */
+	math::Matrix _K;					/**< diagonal gain matrix for position error */
+	math::Matrix _K_rate;			/**< diagonal gain matrix for angular rate error */
 
 	struct {
-		param_t yaw_p;
-		param_t yaw_i;
-		param_t yaw_d;
-		param_t yaw_imax;
-
 		param_t att_p;
-		param_t att_i;
-		param_t att_d;
-		param_t att_imax;
-
 		param_t att_rate_p;
-
+		param_t yaw_p;
 		param_t yaw_rate_p;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
-
-	ECL_MCAttControlVector				_att_control;
-
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -187,11 +163,6 @@ private:
 	void		vehicle_manual_poll();
 
 	/**
-	 * Check for accel updates.
-	 */
-	void		vehicle_accel_poll();
-
-	/**
 	 * Check for set triplet updates.
 	 */
 	void		vehicle_setpoint_poll();
@@ -231,7 +202,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 
 /* subscriptions */
 	_att_sub(-1),
-	_accel_sub(-1),
 	_control_mode_sub(-1),
 	_params_sub(-1),
 	_manual_sub(-1),
@@ -244,19 +214,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
 /* states */
-	_setpoint_valid(false)
-{
-	_parameter_handles.yaw_p	=	param_find("MC_YAWPOS_P");
-	_parameter_handles.yaw_i	=	param_find("MC_YAWPOS_I");
-	_parameter_handles.yaw_d	=	param_find("MC_YAWPOS_D");
-	_parameter_handles.yaw_imax	=	param_find("MC_YAWPOS_IMAX");
+	_setpoint_valid(false),
+/* gain matrices */
+	_K(3, 3),
+	_K_rate(3, 3)
 
+{
 	_parameter_handles.att_p	= 	param_find("MC_ATT_P");
-	_parameter_handles.att_i	= 	param_find("MC_ATT_I");
-	_parameter_handles.att_d	= 	param_find("MC_ATT_D");
-	_parameter_handles.att_imax	= 	param_find("MC_ATT_IMAX");
-
 	_parameter_handles.att_rate_p	= 	param_find("MC_ATTRATE_P");
+	_parameter_handles.yaw_p	=	param_find("MC_YAWPOS_P");
 	_parameter_handles.yaw_rate_p	= 	param_find("MC_YAWRATE_P");
 
 	/* fetch initial parameter values */
@@ -291,29 +257,22 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl()
 int
 MulticopterAttitudeControl::parameters_update()
 {
-
-	//param_get(_parameter_handles.tconst, &(_parameters.tconst));
-
-	param_get(_parameter_handles.yaw_p, &(_parameters.yaw_p));
-	param_get(_parameter_handles.yaw_i, &(_parameters.yaw_i));
-	param_get(_parameter_handles.yaw_d, &(_parameters.yaw_d));
-	param_get(_parameter_handles.yaw_imax, &(_parameters.yaw_imax));
-
-	param_get(_parameter_handles.att_p, &(_parameters.att_p));
-	param_get(_parameter_handles.att_i, &(_parameters.att_i));
-	param_get(_parameter_handles.att_d, &(_parameters.att_d));
-	param_get(_parameter_handles.att_imax, &(_parameters.att_imax));
-
-	param_get(_parameter_handles.yaw_rate_p, &(_parameters.yaw_rate_p));
-
-	param_get(_parameter_handles.att_rate_p, &(_parameters.att_rate_p));
-
-	/* att control parameters */
-	_att_control.set_imax(_parameters.att_imax);
-	//_att_control.set_tau(_parameters.p_tconst);
-	// _att_control.set_k_p(math::radians(_parameters.p_p));
-	// _att_control.set_k_i(math::radians(_parameters.p_i));
-	// _att_control.set_k_d(math::radians(_parameters.p_d));
+	float att_p;
+	float att_rate_p;
+	float yaw_p;
+	float yaw_rate_p;
+
+	param_get(_parameter_handles.att_p, &att_p);
+	param_get(_parameter_handles.att_rate_p, &att_rate_p);
+	param_get(_parameter_handles.yaw_p, &yaw_p);
+	param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p);
+
+	_K(0, 0) = att_p;
+	_K(1, 1) = att_p;
+	_K(2, 2) = yaw_p;
+	_K_rate(0, 0) = att_rate_p;
+	_K_rate(1, 1) = att_rate_p;
+	_K_rate(2, 2) = yaw_rate_p;
 
 	return OK;
 }
@@ -347,18 +306,6 @@ MulticopterAttitudeControl::vehicle_manual_poll()
 }
 
 void
-MulticopterAttitudeControl::vehicle_accel_poll()
-{
-	/* check if there is a new position */
-	bool accel_updated;
-	orb_check(_accel_sub, &accel_updated);
-
-	if (accel_updated) {
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
-	}
-}
-
-void
 MulticopterAttitudeControl::vehicle_setpoint_poll()
 {
 	/* check if there is a new setpoint */
@@ -402,7 +349,6 @@ MulticopterAttitudeControl::task_main()
 	 */
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -418,7 +364,6 @@ MulticopterAttitudeControl::task_main()
 
 	/* get an initial update for all sensor and status data */
 	vehicle_setpoint_poll();
-	vehicle_accel_poll();
 	vehicle_control_mode_poll();
 	vehicle_manual_poll();
 	arming_status_poll();
@@ -451,11 +396,10 @@ MulticopterAttitudeControl::task_main()
 
 		/* only update parameters if they changed */
 		if (fds[0].revents & POLLIN) {
-			/* read from param to clear updated flag */
+			/* copy the topic to clear updated flag */
 			struct parameter_update_s update;
 			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
 
-			/* update parameters from storage */
 			parameters_update();
 		}
 
@@ -469,29 +413,20 @@ MulticopterAttitudeControl::task_main()
 			if (deltaT > 1.0f)
 				deltaT = 0.01f;
 
-			/* load local copies */
+			/* copy attitude topic */
 			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
 
 			vehicle_setpoint_poll();
-			vehicle_accel_poll();
 			vehicle_control_mode_poll();
 			arming_status_poll();
 			vehicle_manual_poll();
 
-			/* lock integrator until armed */
-			if (_arming.armed) {
-				_att_control.lock_integrator(false);
-			} else {
-				_att_control.reset_integral();
-				_att_control.lock_integrator(true);
-			}
-
-			/* decide if in auto or full manual control */
 			float roll_sp = 0.0f;
 			float pitch_sp = 0.0f;
 			float throttle_sp = 0.0f;
 			float yaw_sp = 0.0f;
 
+			/* decide if in auto or manual control */
 			if (_control_mode.flag_control_manual_enabled) {
 				roll_sp = _manual.roll;
 				pitch_sp = _manual.pitch;
@@ -505,51 +440,42 @@ MulticopterAttitudeControl::task_main()
 				throttle_sp = _att_sp.thrust;
 			}
 
-			// XXX take rotation matrix directly from att_sp for auto mode
-			math::Vector3 F_des(roll_sp, pitch_sp, yaw_sp);
-
-			math::Vector3 rates_des;
-
-			math::Dcm R_nb(_att.R);
-			// XXX actually it's not euler angles rates, but body rates, rename it (?)
-			math::Vector3 angular_rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed);
+			/* construct rotation matrix from euler angles */
+			math::EulerAngles euler(roll_sp, pitch_sp, yaw_sp);
+			math::Dcm R_des(euler);
 
-			_att_control.control(deltaT, R_nb, _att.yaw, F_des,
-                                _parameters.att_p, _parameters.att_d, _parameters.att_i,
-                                angular_rates, rates_des, throttle_sp);
+			/* rotation matrix for current state */
+			math::Dcm R(_att.R);
+			/* current body angular rates */
+			math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed);
 
-			float roll_out = _parameters.att_rate_p * rates_des(0);
-			float pitch_out = _parameters.att_rate_p * rates_des(1);
-			float yaw_out = _parameters.yaw_rate_p * rates_des(2);
-			
-			_actuators.control[0] = (isfinite(roll_out)) ? roll_out : 0.0f;
-			_actuators.control[1] = (isfinite(pitch_out)) ? pitch_out : 0.0f;
-			_actuators.control[2] = (isfinite(yaw_out)) ? yaw_out : 0.0f;
+			/* orientation error between R and R_des */
+			math::Matrix e_R_m = (R_des.transpose() * R - R.transpose() * R_des).transpose() * 0.5f;
+			math::Vector3 e_R(-e_R_m(1, 2), e_R_m(0, 2), -e_R_m(0, 1));
 
-			/* throttle passed through */
-			_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
-
-			/*
-			 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
-			 * only once available
-			 */
-			vehicle_rates_setpoint_s rates_sp;
-			rates_sp.roll = rates_des(0);
-			rates_sp.pitch = rates_des(1);
-			rates_sp.yaw = rates_des(2);
+			/* angular rates setpoint*/
+			math::Vector3 rates_sp = K_p * e_R;
+			math::Vector3 control = K_r_p * (rates_sp - rates);
 
-			rates_sp.timestamp = hrt_absolute_time();
+			/* publish the attitude rates setpoint */
+			_rates_setpoint.roll = rates_sp(0);
+			_rates_setpoint.pitch = rates_sp(1);
+			_rates_setpoint.yaw = rates_sp(2);
+			_rates_setpoint.thrust = throttle_sp;
+			_rates_setpoint.timestamp = hrt_absolute_time();
 
 			if (_rate_sp_pub > 0) {
-				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(vehicle_rates_setpoint), _actuators_0_pub, &rates_sp);
+				orb_publish(ORB_ID(vehicle_rates_setpoint), _actuators_0_pub, &_rates_setpoint);
 
 			} else {
-				/* advertise and publish */
-				_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+				_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_setpoint);
 			}
 
-			/* lazily publish the setpoint only once available */
+			/* publish the attitude controls */
+			_actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f;
+			_actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f;
+			_actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f;
+			_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
 			_actuators.timestamp = hrt_absolute_time();
 
 			if (_actuators_0_pub > 0) {
@@ -566,7 +492,7 @@ MulticopterAttitudeControl::task_main()
 		perf_end(_loop_perf);
 	}
 
-	warnx("exiting.\n");
+	warnx("exit");
 
 	_control_task = -1;
 	_exit(0);
diff --git a/src/modules/mc_att_control_vector/module.mk b/src/modules/mc_att_control_vector/module.mk
index 61eb3b3fd..8d380d719 100644
--- a/src/modules/mc_att_control_vector/module.mk
+++ b/src/modules/mc_att_control_vector/module.mk
@@ -37,5 +37,4 @@
 
 MODULE_COMMAND	= mc_att_control_vector
 
-SRCS		= mc_att_control_vector_main.cpp \
-		  ecl_mc_att_control_vector.cpp
+SRCS		= mc_att_control_vector_main.cpp