added VTOL attitude control module

2 files changed, 756 insertions, 0 deletions

diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk
new file mode 100644
index 000000000..c349c2340
--- /dev/null
+++ b/src/modules/vtol_att_control/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2013, 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.
+#
+############################################################################
+
+#
+# VTOL attitude controller
+#
+
+MODULE_COMMAND	= vtol_att_control
+
+SRCS		= vtol_att_control_main.cpp
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
new file mode 100644
index 000000000..62b6a4ed6
--- /dev/null
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -0,0 +1,716 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 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 VTOL_att_control_main.cpp
+ * Implementation of an attitude controller for VTOL airframes. This module receives data
+ * from both the fixed wing- and the multicopter attitude controllers and processes it.
+ * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward-
+ * flight or transition). It also publishes the resulting controls on the actuator controls topics.
+ *
+ * @author Roman Bapst 		<bapstr@ethz.ch>
+ * @author Lorenz Meier 	<lm@inf.ethz.ch>
+ * @author Thomas Gubler	<thomasgubler@gmail.com>
+ *
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.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/vtol_vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/circuit_breaker.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+
+#include "drivers/drv_pwm_output.h"
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <nuttx/mtd.h>
+
+#include <fcntl.h>
+
+
+extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
+
+class VtolAttitudeControl
+{
+public:
+
+	VtolAttitudeControl();
+	~VtolAttitudeControl();
+
+	int start();	/* start the task and return OK on success */
+
+
+private:
+//******************flags & handlers******************************************************
+	bool _task_should_exit;
+	int _control_task;		//task handle for VTOL attitude controller
+
+	/* handlers for subscriptions */
+	int		_v_att_sub;				//vehicle attitude subscription
+	int		_v_att_sp_sub;			//vehicle attitude setpoint subscription
+	int		_mc_virtual_v_rates_sp_sub;		//vehicle rates setpoint subscription
+	int		_fw_virtual_v_rates_sp_sub;		//vehicle rates setpoint subscription
+	int		_v_control_mode_sub;	//vehicle control mode subscription
+	int		_params_sub;			//parameter updates subscription
+	int		_manual_control_sp_sub;	//manual control setpoint subscription
+	int		_armed_sub;				//arming status subscription
+
+	int 	_actuator_inputs_mc;	//topic on which the mc_att_controller publishes actuator inputs
+	int 	_actuator_inputs_fw;	//topic on which the fw_att_controller publishes actuator inputs
+
+	//handlers for publishers
+	orb_advert_t	_actuators_0_pub;		//input for the mixer (roll,pitch,yaw,thrust)
+	orb_advert_t 	_actuators_1_pub;
+	orb_advert_t	_vtol_vehicle_status_pub;
+	orb_advert_t	_v_rates_sp_pub;
+//*******************data containers***********************************************************
+	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 vehicle_rates_setpoint_s		_mc_virtual_v_rates_sp;		// virtual mc vehicle rates setpoint
+	struct vehicle_rates_setpoint_s		_fw_virtual_v_rates_sp;		// virtual fw 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 vtol_vehicle_status_s 		_vtol_vehicle_status;
+	struct actuator_controls_s			_actuators_out_0;	//actuator controls going to the mc mixer
+	struct actuator_controls_s			_actuators_out_1;	//actuator controls going to the fw mixer (used for elevons)
+	struct actuator_controls_s			_actuators_mc_in;	//actuator controls from mc_att_control
+	struct actuator_controls_s			_actuators_fw_in;	//actuator controls from fw_att_control
+	struct actuator_armed_s				_armed;				//actuator arming status
+
+	struct {
+		float min_pwm_mc;	//pwm value for idle in mc mode
+	} _params;
+
+	struct {
+		param_t min_pwm_mc;
+	} _params_handles;
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	/* for multicopters it is usual to have a non-zero idle speed of the engines
+	 * for fixed wings we want to have an idle speed of zero since we do not want
+	 * to waste energy when gliding. */
+	bool flag_idle_mc;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
+
+//*****************Member functions***********************************************************************
+
+	void 		task_main();	//main task
+	static void	task_main_trampoline(int argc, char *argv[]);	//Shim for calling task_main from task_create.
+
+	void		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
+	void		vehicle_manual_poll();			//Check for changes in manual inputs.
+	void		arming_status_poll();			//Check for arming status updates.
+	void 		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
+	void 		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
+	void 		vehicle_rates_sp_mc_poll();
+	void 		vehicle_rates_sp_fw_poll();
+	void 		parameters_update_poll();		//Check if parameters have changed
+	int 		parameters_update();			//Update local paraemter cache
+	void  		fill_mc_att_control_output();	//write mc_att_control results to actuator message
+	void		fill_fw_att_control_output();	//write fw_att_control results to actuator message
+	void 		fill_mc_att_rates_sp();
+	void 		fill_fw_att_rates_sp();
+	void 		set_idle_fw();
+	void 		set_idle_mc();
+};
+
+namespace VTOL_att_control
+{
+VtolAttitudeControl *g_control;
+}
+
+/**
+* Constructor
+*/
+VtolAttitudeControl::VtolAttitudeControl() :
+	_task_should_exit(false),
+	_control_task(-1),
+
+	//init subscription handlers
+	_v_att_sub(-1),
+	_v_att_sp_sub(-1),
+	_mc_virtual_v_rates_sp_sub(-1),
+	_fw_virtual_v_rates_sp_sub(-1),
+	_v_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_control_sp_sub(-1),
+	_armed_sub(-1),
+
+	//init publication handlers
+	_actuators_0_pub(-1),
+	_actuators_1_pub(-1),
+	_vtol_vehicle_status_pub(-1),
+	_v_rates_sp_pub(-1),
+
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+{
+
+	flag_idle_mc = true;	/*assume we always start in mc mode for a VTOL airframe */
+
+	memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
+	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/
+	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(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp));
+	memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp));
+	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
+	memset(&_actuators_out_0, 0, sizeof(_actuators_out_0));
+	memset(&_actuators_out_1, 0, sizeof(_actuators_out_1));
+	memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
+	memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
+	memset(&_armed, 0, sizeof(_armed));
+
+	_params_handles.min_pwm_mc = param_find("PWM_MIN");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+/**
+* Destructor
+*/
+VtolAttitudeControl::~VtolAttitudeControl()
+{
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	VTOL_att_control::g_control = nullptr;
+}
+
+/**
+* Check for changes in vehicle control mode.
+*/
+void VtolAttitudeControl::vehicle_control_mode_poll()
+{
+	bool updated;
+
+	/* Check if vehicle control mode has changed */
+	orb_check(_v_control_mode_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
+	}
+}
+
+/**
+* Check for changes in manual inputs.
+*/
+void VtolAttitudeControl::vehicle_manual_poll()
+{
+	bool updated;
+
+	/* get pilots inputs */
+	orb_check(_manual_control_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
+	}
+}
+/**
+* Check for arming status updates.
+*/
+void VtolAttitudeControl::arming_status_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_armed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+	}
+}
+
+/**
+* Check for inputs from mc attitude controller.
+*/
+void VtolAttitudeControl::actuator_controls_mc_poll()
+{
+	bool updated;
+	orb_check(_actuator_inputs_mc, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in);
+	}
+}
+
+/**
+* Check for inputs from fw attitude controller.
+*/
+void VtolAttitudeControl::actuator_controls_fw_poll()
+{
+	bool updated;
+	orb_check(_actuator_inputs_fw, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in);
+	}
+}
+
+/**
+* Check for attitude rates setpoint from mc attitude controller
+*/
+void VtolAttitudeControl::vehicle_rates_sp_mc_poll()
+{
+	bool updated;
+	orb_check(_mc_virtual_v_rates_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub , &_mc_virtual_v_rates_sp);
+	}
+}
+
+/**
+* Check for attitude rates setpoint from fw attitude controller
+*/
+void VtolAttitudeControl::vehicle_rates_sp_fw_poll()
+{
+	bool updated;
+	orb_check(_fw_virtual_v_rates_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub , &_fw_virtual_v_rates_sp);
+	}
+}
+
+/**
+* Check for parameter updates.
+*/
+void
+VtolAttitudeControl::parameters_update_poll()
+{
+	bool updated;
+
+	/* Check if parameters have changed */
+	orb_check(_params_sub, &updated);
+
+	if (updated) {
+		struct parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+		parameters_update();
+	}
+}
+
+/**
+* Update parameters.
+*/
+int
+VtolAttitudeControl::parameters_update()
+{
+	/* idle pwm */
+	float v;
+	param_get(_params_handles.min_pwm_mc, &v);
+	_params.min_pwm_mc = v;
+
+	return OK;
+}
+
+/**
+* Prepare message to acutators with data from mc attitude controller.
+*/
+void VtolAttitudeControl::fill_mc_att_control_output()
+{
+	_actuators_out_0.control[0] = _actuators_mc_in.control[0];
+	_actuators_out_0.control[1] = _actuators_mc_in.control[1];
+	_actuators_out_0.control[2] = _actuators_mc_in.control[2]; 
+	_actuators_out_0.control[3] = _actuators_mc_in.control[3];
+	//set neutral position for elevons
+	_actuators_out_1.control[0] = _actuators_mc_in.control[2];	//roll elevon
+	_actuators_out_1.control[1] = _actuators_mc_in.control[1];;	//pitch elevon
+}
+
+/**
+* Prepare message to acutators with data from fw attitude controller.
+*/
+void VtolAttitudeControl::fill_fw_att_control_output()
+{
+	/*For the first test in fw mode, only use engines for thrust!!!*/
+	_actuators_out_0.control[0] = 0;
+	_actuators_out_0.control[1] = 0;
+	_actuators_out_0.control[2] = 0;
+	_actuators_out_0.control[3] = _actuators_fw_in.control[3];
+	/*controls for the elevons */
+	_actuators_out_1.control[0] = -_actuators_fw_in.control[0];	/*roll elevon*/
+	_actuators_out_1.control[1] = _actuators_fw_in.control[1];	/*pitch elevon */
+}
+
+/**
+* Prepare message for mc attitude rates setpoint topic
+*/
+void VtolAttitudeControl::fill_mc_att_rates_sp()
+{
+	_v_rates_sp.roll 	= _mc_virtual_v_rates_sp.roll;
+	_v_rates_sp.pitch 	= _mc_virtual_v_rates_sp.pitch;
+	_v_rates_sp.yaw 	= _mc_virtual_v_rates_sp.yaw;
+	_v_rates_sp.thrust 	= _mc_virtual_v_rates_sp.thrust;
+}
+
+/**
+* Prepare message for fw attitude rates setpoint topic
+*/
+void VtolAttitudeControl::fill_fw_att_rates_sp()
+{
+	_v_rates_sp.roll 	= _fw_virtual_v_rates_sp.roll;
+	_v_rates_sp.pitch 	= _fw_virtual_v_rates_sp.pitch;
+	_v_rates_sp.yaw 	= _fw_virtual_v_rates_sp.yaw;
+	_v_rates_sp.thrust 	= _fw_virtual_v_rates_sp.thrust;
+}
+
+/**
+* Adjust idle speed for fw mode.
+*/
+void VtolAttitudeControl::set_idle_fw()
+{
+	int ret;
+	char *dev = PWM_OUTPUT_DEVICE_PATH;
+	int fd = open(dev, 0);
+
+	if (fd < 0) {err(1, "can't open %s", dev);}
+
+	unsigned pwm_value = PWM_LOWEST_MIN;
+	struct pwm_output_values pwm_values;
+	memset(&pwm_values, 0, sizeof(pwm_values));
+
+	for (unsigned i = 0; i < 4; i++) {
+
+		pwm_values.values[i] = pwm_value;
+		pwm_values.channel_count++;
+	}
+
+	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {errx(ret, "failed setting min values");}
+
+	close(fd);
+}
+
+/**
+* Adjust idle speed for mc mode.
+*/
+void VtolAttitudeControl::set_idle_mc()
+{
+	int ret;
+	unsigned servo_count;
+	char *dev = PWM_OUTPUT_DEVICE_PATH;
+	int fd = open(dev, 0);
+
+	if (fd < 0) {err(1, "can't open %s", dev);}
+
+	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	unsigned pwm_value = 1100;
+	struct pwm_output_values pwm_values;
+	memset(&pwm_values, 0, sizeof(pwm_values));
+
+	for (unsigned i = 0; i < 4; i++) {
+		pwm_values.values[i] = pwm_value;
+		pwm_values.channel_count++;
+	}
+
+	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {errx(ret, "failed setting min values");}
+
+	close(fd);
+}
+
+void
+VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
+{
+	VTOL_att_control::g_control->task_main();
+}
+
+void VtolAttitudeControl::task_main()
+{
+	warnx("started");
+	fflush(stdout);
+
+	/* do subscriptions */
+	_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
+	_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
+	_v_att_sub             = orb_subscribe(ORB_ID(vehicle_attitude));
+	_v_control_mode_sub    = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_params_sub            = orb_subscribe(ORB_ID(parameter_update));
+	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_armed_sub             = orb_subscribe(ORB_ID(actuator_armed));
+
+	_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
+	_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
+
+	parameters_update();  /*initialize parameter cache/*
+
+	/* wakeup source*/
+	struct pollfd fds[3];	/*input_mc, input_fw, parameters*/
+
+	fds[0].fd     = _actuator_inputs_mc;
+	fds[0].events = POLLIN;
+	fds[1].fd     = _actuator_inputs_fw;
+	fds[1].events = POLLIN;
+	fds[2].fd     = _params_sub;
+	fds[2].events = POLLIN;
+
+	while (!_task_should_exit) {
+		/*Advertise/Publish vtol vehicle status*/
+		if (_vtol_vehicle_status_pub > 0) {
+			orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
+
+		} else {
+			_vtol_vehicle_status.timestamp = hrt_absolute_time();
+			_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
+		}
+
+		/* wait for up to 100ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+
+		/* timed out - periodic check for _task_should_exit */
+		if (pret == 0) {
+			continue;
+		}
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			/* sleep a bit before next try */
+			usleep(100000);
+			continue;
+		}
+
+		if (fds[2].revents & POLLIN) {	//parameters were updated, read them now
+			/* read from param to clear updated flag */
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+			/* update parameters from storage */
+			parameters_update();
+		}
+
+		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
+		vehicle_manual_poll();			//Check for changes in manual inputs.
+		arming_status_poll();			//Check for arming status updates.
+		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
+		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
+		vehicle_rates_sp_mc_poll();
+		vehicle_rates_sp_fw_poll();
+		parameters_update_poll();
+
+		if (_manual_control_sp.aux1 <= 0.0f) {		/* vehicle is in mc mode */
+			_vtol_vehicle_status.vtol_in_rw_mode = true;
+
+			if (!flag_idle_mc) {	/* we want to adjust idle speed for mc mode */
+				set_idle_mc();
+				flag_idle_mc = true;
+			}
+
+			/* got data from mc_att_controller */
+			if (fds[0].revents & POLLIN) {
+				vehicle_manual_poll();	/* update remote input */
+				orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
+
+				fill_mc_att_control_output();
+				fill_mc_att_rates_sp();
+
+				if (_actuators_0_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+
+				} else {
+					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+				}
+
+				if (_actuators_1_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+
+				} else {
+					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+				}
+			}
+		}
+
+		if (_manual_control_sp.aux1 >= 0.0f) {			/* vehicle is in fw mode */
+			_vtol_vehicle_status.vtol_in_rw_mode = false;
+
+			if (flag_idle_mc) {	/* we want to adjust idle speed for fixed wing mode */
+				set_idle_fw();
+				flag_idle_mc = false;
+			}
+
+			if (fds[1].revents & POLLIN) {		/* got data from fw_att_controller */
+				orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
+				vehicle_manual_poll();	//update remote input
+
+				fill_fw_att_control_output();
+				fill_fw_att_rates_sp();
+
+				if (_actuators_0_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+
+				} else {
+					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+				}
+
+				if (_actuators_1_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+
+				} else {
+					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+				}
+			}
+		}
+
+		// publish the attitude rates setpoint
+		if(_v_rates_sp_pub > 0) {
+			orb_publish(ORB_ID(vehicle_rates_setpoint),_v_rates_sp_pub,&_v_rates_sp);
+		}
+		else {
+			_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),&_v_rates_sp);
+		}
+	}
+
+	warnx("exit");
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+VtolAttitudeControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("vtol_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 10,
+				       2048,
+				       (main_t)&VtolAttitudeControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+
+int vtol_att_control_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: vtol_att_control {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (VTOL_att_control::g_control != nullptr) {
+			errx(1, "already running");
+		}
+
+		VTOL_att_control::g_control = new VtolAttitudeControl;
+
+		if (VTOL_att_control::g_control == nullptr) {
+			errx(1, "alloc failed");
+		}
+
+		if (OK != VTOL_att_control::g_control->start()) {
+			delete VTOL_att_control::g_control;
+			VTOL_att_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (VTOL_att_control::g_control == nullptr) {
+			errx(1, "not running");
+		}
+
+		delete VTOL_att_control::g_control;
+		VTOL_att_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (VTOL_att_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}