adapted attitude controllers to support VTOL

2 files changed, 191 insertions, 58 deletions

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index e770c11a2..daf787863 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -35,8 +35,9 @@
  * @file fw_att_control_main.c
  * Implementation of a generic attitude controller based on classic orthogonal PIDs.
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Lorenz Meier 	<lm@inf.ethz.ch>
+ * @author Thomas Gubler 	<thomasgubler@gmail.com>
+ * @author Roman Bapst		<bapstr@ethz.ch>
  *
  */
 
@@ -76,6 +77,7 @@
 #include <ecl/attitude_fw/ecl_roll_controller.h>
 #include <ecl/attitude_fw/ecl_yaw_controller.h>
 
+
 /**
  * Fixedwing attitude control app start / stop handling function
  *
@@ -92,12 +94,12 @@ public:
 	FixedwingAttitudeControl();
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor, also kills the main task.
 	 */
 	~FixedwingAttitudeControl();
 
 	/**
-	 * Start the sensors task.
+	 * Start the main task.
 	 *
 	 * @return	OK on success.
 	 */
@@ -112,9 +114,9 @@ public:
 
 private:
 
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		_task_should_exit;		/**< if true, attitude control task should exit */
 	bool		_task_running;			/**< if true, task is running in its mainloop */
-	int		_control_task;			/**< task handle for sensor task */
+	int		_control_task;			/**< task handle */
 
 	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_accel_sub;			/**< accelerometer subscription */
@@ -128,13 +130,16 @@ private:
 	int		_vehicle_status_sub;		/**< vehicle status subscription */
 
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
+	orb_advert_t 	_rate_sp_virtual_pub;	/* virtual att rates setpoint topic (vtol) */
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
-	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
+	orb_advert_t	_actuators_virtual_fw_pub; /**publisher for VTOL vehicle*/
+	orb_advert_t	_actuators_2_pub;		/**< actuator control group 1 setpoint (Airframe) */
 
 	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 vehicle_rates_setpoint_s			_rates_sp;	/* attitude rates 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 */
@@ -189,6 +194,8 @@ private:
 		float man_roll_max;						/**< Max Roll in rad */
 		float man_pitch_max;					/**< Max Pitch in rad */
 
+		param_t autostart_id;			/* indicates which airframe is used */
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -228,6 +235,8 @@ private:
 		param_t pitchsp_offset_deg;
 		param_t man_roll_max;
 		param_t man_pitch_max;
+
+		param_t autostart_id;		/* indicates which airframe is used */
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -289,7 +298,7 @@ private:
 	static void	task_main_trampoline(int argc, char *argv[]);
 
 	/**
-	 * Main sensor collection task.
+	 * Main attitude controller collection task.
 	 */
 	void		task_main();
 
@@ -325,9 +334,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 /* publications */
 	_rate_sp_pub(-1),
+	_rate_sp_virtual_pub(-1),
 	_attitude_sp_pub(-1),
 	_actuators_0_pub(-1),
-	_actuators_1_pub(-1),
+	_actuators_virtual_fw_pub(-1),
+	_actuators_2_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -341,6 +352,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_att = {};
 	_accel = {};
 	_att_sp = {};
+	_rates_sp = {};
 	_manual = {};
 	_airspeed = {};
 	_vcontrol_mode = {};
@@ -386,6 +398,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
 	_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
 
+	_parameter_handles.autostart_id 	= param_find("SYS_AUTOSTART");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -462,6 +476,7 @@ FixedwingAttitudeControl::parameters_update()
 	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
 	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
 
+	param_get(_parameter_handles.autostart_id, &_parameters.autostart_id);
 
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
@@ -497,7 +512,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 {
 	bool vcontrol_mode_updated;
 
-	/* Check HIL state if vehicle status has changed */
+	/* Check if vehicle control mode has changed */
 	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
 
 	if (vcontrol_mode_updated) {
@@ -529,7 +544,6 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
 
 	if (airspeed_updated) {
 		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
-//		warnx("airspeed poll: ind: %.4f,  true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
 	}
 }
 
@@ -616,6 +630,18 @@ FixedwingAttitudeControl::task_main()
 
 	parameters_update();
 
+	/*Publish to correct actuator control topic, depending on what airframe we are using
+	 * If airframe is of type VTOL then we want to publish the actuator controls on the virtual fixed wing
+	 * topic, from which the vtol_att_control module is receiving data and processing it further)*/
+	if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) {	/* VTOL airframe?*/
+		_actuators_virtual_fw_pub = orb_advertise(ORB_ID(actuator_controls_virtual_fw), &_actuators);
+		_rate_sp_virtual_pub      = orb_advertise(ORB_ID(fw_virtual_rates_setpoint), &_rates_sp);
+
+	} else {	/*airframe is not of type VTOL, use standard topic for controls publication*/
+		_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+		_rate_sp_pub     = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
+	}
+
 	/* get an initial update for all sensor and status data */
 	vehicle_airspeed_poll();
 	vehicle_setpoint_poll();
@@ -679,6 +705,65 @@ FixedwingAttitudeControl::task_main()
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
 
+			if (_parameters.autostart_id >= 13000
+			    && _parameters.autostart_id <= 13999) {	//vehicle type is VTOL, need to modify attitude!
+				/* The following modification to the attitude is vehicle specific and in this case applies
+				   to tail-sitter models !!!
+
+				 * Since the VTOL airframe is initialized as a multicopter we need to
+				 * modify the estimated attitude for the fixed wing operation.
+				 * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
+				 * the pitch axis compared to the neutral position of the vehicle in multicopter mode
+				 * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
+				 * Additionally, in order to get the correct sign of the pitch, we need to multiply
+				 * the new x axis of the rotation matrix with -1
+				 *
+				 * original:			modified:
+				 *
+				 * Rxx  Ryx  Rzx		-Rzx  Ryx  Rxx
+				 * Rxy	Ryy  Rzy		-Rzy  Ryy  Rxy
+				 * Rxz	Ryz  Rzz		-Rzz  Ryz  Rxz
+				 * */
+				math::Matrix<3, 3> R;				//original rotation matrix
+				math::Matrix<3, 3> R_adapted;		//modified rotation matrix
+				R.set(_att.R);
+				R_adapted.set(_att.R);
+
+				//move z to x
+				R_adapted(0, 0) = R(0, 2);
+				R_adapted(1, 0) = R(1, 2);
+				R_adapted(2, 0) = R(2, 2);
+				//move x to z
+				R_adapted(0, 2) = R(0, 0);
+				R_adapted(1, 2) = R(1, 0);
+				R_adapted(2, 2) = R(2, 0);
+
+				//change direction of pitch (convert to right handed system)
+				R_adapted(0, 0) = -R_adapted(0, 0);
+				R_adapted(1, 0) = -R_adapted(1, 0);
+				R_adapted(2, 0) = -R_adapted(2, 0);
+				math::Vector<3> euler_angles;		//adapted euler angles for fixed wing operation
+				euler_angles = R_adapted.to_euler();
+				//fill in new attitude data
+				_att.roll    = euler_angles(0);
+				_att.pitch   = euler_angles(1);
+				_att.yaw     = euler_angles(2);
+				_att.R[0][0] = R_adapted(0, 0);
+				_att.R[0][1] = R_adapted(0, 1);
+				_att.R[0][2] = R_adapted(0, 2);
+				_att.R[1][0] = R_adapted(1, 0);
+				_att.R[1][1] = R_adapted(1, 1);
+				_att.R[1][2] = R_adapted(1, 2);
+				_att.R[2][0] = R_adapted(2, 0);
+				_att.R[2][1] = R_adapted(2, 1);
+				_att.R[2][2] = R_adapted(2, 2);
+
+				// lastly, roll- and yawspeed have to be swaped
+				float helper = _att.rollspeed;
+				_att.rollspeed = -_att.yawspeed;
+				_att.yawspeed = helper;
+			}
+
 			vehicle_airspeed_poll();
 
 			vehicle_setpoint_poll();
@@ -696,7 +781,7 @@ FixedwingAttitudeControl::task_main()
 			/* lock integrator until control is started */
 			bool lock_integrator;
 
-			if (_vcontrol_mode.flag_control_attitude_enabled) {
+			if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) {
 				lock_integrator = false;
 
 			} else {
@@ -705,10 +790,10 @@ FixedwingAttitudeControl::task_main()
 
 			/* Simple handling of failsafe: deploy parachute if failsafe is on */
 			if (_vcontrol_mode.flag_control_termination_enabled) {
-				_actuators_airframe.control[1] = 1.0f;
+				_actuators_airframe.control[7] = 1.0f;
 //				warnx("_actuators_airframe.control[1] = 1.0f;");
 			} else {
-				_actuators_airframe.control[1] = 0.0f;
+				_actuators_airframe.control[7] = 0.0f;
 //				warnx("_actuators_airframe.control[1] = -1.0f;");
 			}
 
@@ -802,18 +887,18 @@ FixedwingAttitudeControl::task_main()
 					att_sp.thrust = throttle_sp;
 
 					/* lazily publish the setpoint only once available */
-					if (_attitude_sp_pub > 0) {
+					if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) {
 						/* publish the attitude setpoint */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
 
-					} else {
+					} else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) {
 						/* advertise and publish */
 						_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 					}
 				}
 
 				/* If the aircraft is on ground reset the integrators */
-				if (_vehicle_status.condition_landed) {
+				if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) {
 					_roll_ctrl.reset_integrator();
 					_pitch_ctrl.reset_integrator();
 					_yaw_ctrl.reset_integrator();
@@ -915,20 +1000,19 @@ FixedwingAttitudeControl::task_main()
 				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
 				 * only once available
 				 */
-				vehicle_rates_setpoint_s rates_sp;
-				rates_sp.roll = _roll_ctrl.get_desired_rate();
-				rates_sp.pitch = _pitch_ctrl.get_desired_rate();
-				rates_sp.yaw = _yaw_ctrl.get_desired_rate();
+				_rates_sp.roll = _roll_ctrl.get_desired_rate();
+				_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
+				_rates_sp.yaw = _yaw_ctrl.get_desired_rate();
 
-				rates_sp.timestamp = hrt_absolute_time();
+				_rates_sp.timestamp = hrt_absolute_time();
 
 				if (_rate_sp_pub > 0) {
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+					/* publish the attitude rates setpoint */
+					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
 
-				} else {
-					/* advertise and publish */
-					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+				} else if (_rate_sp_virtual_pub > 0) {
+					/* publish the virtual attitude setpoint */
+					orb_publish(ORB_ID(fw_virtual_rates_setpoint), _rate_sp_virtual_pub, &_rates_sp);
 				}
 
 			} else {
@@ -948,28 +1032,22 @@ FixedwingAttitudeControl::task_main()
 			_actuators.timestamp = hrt_absolute_time();
 			_actuators_airframe.timestamp = hrt_absolute_time();
 
-			if (_actuators_0_pub > 0) {
-				/* publish the attitude setpoint */
+			/* publish the actuator controls */
+			if (_actuators_0_pub > 0) {		//normal fixed wing airframe
 				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
 
-			} else {
-				/* advertise and publish */
-				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+			} else if (_actuators_0_pub < 0 && !_vehicle_status.is_rotary_wing) {		//VTOL airframe
+				orb_publish(ORB_ID(actuator_controls_virtual_fw), _actuators_virtual_fw_pub, &_actuators);
 			}
 
-			if (_actuators_1_pub > 0) {
-				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
-//				warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
-//						(double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
-//						(double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
-//						(double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);
+			if (_actuators_2_pub > 0) {
+				/* publish the actuator controls*/
+				orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
 
 			} else {
 				/* advertise and publish */
-				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
+				_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
 			}
-
 		}
 
 		loop_counter++;
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 81a13edb8..c5dccd8c3 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -66,6 +66,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
@@ -96,12 +97,12 @@ public:
 	MulticopterAttitudeControl();
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor, also kills the main task
 	 */
 	~MulticopterAttitudeControl();
 
 	/**
-	 * Start the sensors task.
+	 * Start the multicopter attitude control task.
 	 *
 	 * @return		OK on success.
 	 */
@@ -109,8 +110,8 @@ public:
 
 private:
 
-	bool	_task_should_exit;		/**< if true, sensor task should exit */
-	int		_control_task;			/**< task handle for sensor task */
+	bool	_task_should_exit;		/**< if true, task_main() should exit */
+	int		_control_task;			/**< task handle */
 
 	int		_v_att_sub;				/**< vehicle attitude subscription */
 	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
@@ -119,10 +120,13 @@ private:
 	int		_params_sub;			/**< parameter updates subscription */
 	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
 	int		_armed_sub;				/**< arming status subscription */
+	int		_vehicle_status_sub;	/**< vehicle status subscription */
 
 	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
 	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
+	orb_advert_t 	_v_rates_sp_virtual_pub;	/* virtual att rates sp publication */
 	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
+	orb_advert_t 	_actuators_virtual_mc_pub; /**attitude actuator controls publication if vehicle is VTOL*/
 
 	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
 
@@ -133,6 +137,7 @@ private:
 	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 */
+	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -168,6 +173,8 @@ private:
 		param_t acro_roll_max;
 		param_t acro_pitch_max;
 		param_t acro_yaw_max;
+
+		param_t autostart_id;	//what frame are we using?
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -182,6 +189,8 @@ private:
 		float man_pitch_max;
 		float man_yaw_max;
 		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
+
+		param_t autostart_id;
 	}		_params;
 
 	/**
@@ -230,6 +239,11 @@ private:
 	void		control_attitude_rates(float dt);
 
 	/**
+	 * Check for vehicle status updates.
+	 */
+	void		vehicle_status_poll();
+
+	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -264,11 +278,14 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_sub(-1),
 	_manual_control_sp_sub(-1),
 	_armed_sub(-1),
+	_vehicle_status_sub(-1),
 
 /* publications */
 	_att_sp_pub(-1),
 	_v_rates_sp_pub(-1),
+	_v_rates_sp_virtual_pub(-1),
 	_actuators_0_pub(-1),
+	_actuators_virtual_mc_pub(-1),
 
 	_actuators_0_circuit_breaker_enabled(false),
 
@@ -283,6 +300,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
 	memset(&_actuators, 0, sizeof(_actuators));
 	memset(&_armed, 0, sizeof(_armed));
+	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
+	_vehicle_status.is_rotary_wing = true;
 
 	_params.att_p.zero();
 	_params.rate_p.zero();
@@ -295,6 +314,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.man_yaw_max = 0.0f;
 	_params.acro_rate_max.zero();
 
+	_params.autostart_id = 0; //default
+
 	_rates_prev.zero();
 	_rates_sp.zero();
 	_rates_int.zero();
@@ -324,6 +345,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
 	_params_handles.acro_yaw_max		= 	param_find("MC_ACRO_Y_MAX");
 
+	_params_handles.autostart_id 	= param_find("SYS_AUTOSTART");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -409,6 +432,8 @@ MulticopterAttitudeControl::parameters_update()
 
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
 
+	param_get(_params_handles.autostart_id, &_params.autostart_id);
+
 	return OK;
 }
 
@@ -417,7 +442,7 @@ MulticopterAttitudeControl::parameter_update_poll()
 {
 	bool updated;
 
-	/* Check HIL state if vehicle status has changed */
+	/* Check if parameters have changed */
 	orb_check(_params_sub, &updated);
 
 	if (updated) {
@@ -432,7 +457,7 @@ MulticopterAttitudeControl::vehicle_control_mode_poll()
 {
 	bool updated;
 
-	/* Check HIL state if vehicle status has changed */
+	/* Check if vehicle control mode has changed */
 	orb_check(_v_control_mode_sub, &updated);
 
 	if (updated) {
@@ -489,6 +514,18 @@ MulticopterAttitudeControl::arming_status_poll()
 	}
 }
 
+void
+MulticopterAttitudeControl::vehicle_status_poll()
+{
+	/* check if there is new status information */
+	bool vehicle_status_updated;
+	orb_check(_vehicle_status_sub, &vehicle_status_updated);
+
+	if (vehicle_status_updated) {
+		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+	}
+}
+
 /*
  * Attitude controller.
  * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
@@ -585,7 +622,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	}
 
 	/* publish the attitude setpoint if needed */
-	if (publish_att_sp) {
+	if (publish_att_sp && _vehicle_status.is_rotary_wing) {
 		_v_att_sp.timestamp = hrt_absolute_time();
 
 		if (_att_sp_pub > 0) {
@@ -682,7 +719,7 @@ void
 MulticopterAttitudeControl::control_attitude_rates(float dt)
 {
 	/* reset integral if disarmed */
-	if (!_armed.armed) {
+	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
 		_rates_int.zero();
 	}
 
@@ -721,6 +758,8 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
 void
 MulticopterAttitudeControl::task_main()
 {
+	warnx("started");
+	fflush(stdout);
 
 	/*
 	 * do subscriptions
@@ -732,10 +771,24 @@ MulticopterAttitudeControl::task_main()
 	_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));
+	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/* initialize parameters cache */
 	parameters_update();
 
+	/*Subscribe to correct actuator control topic, depending on what airframe we are using
+	 * If airframe is of type VTOL then we want to publish the actuator controls on the virtual multicopter
+	 * topic, from which the VTOL_att_control module is receiving data and processing it further)*/
+	if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) {	/* VTOL airframe?*/
+		_actuators_virtual_mc_pub = orb_advertise(ORB_ID(actuator_controls_virtual_mc), &_actuators);
+		_v_rates_sp_virtual_pub = orb_advertise(ORB_ID(mc_virtual_rates_setpoint), &_v_rates_sp);
+
+	} else {	/*airframe is not of type VTOL, use standard topic for controls publication*/
+		_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+		_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+	}
+
+
 	/* wakeup source: vehicle attitude */
 	struct pollfd fds[1];
 
@@ -783,6 +836,7 @@ MulticopterAttitudeControl::task_main()
 			vehicle_control_mode_poll();
 			arming_status_poll();
 			vehicle_manual_poll();
+			vehicle_status_poll();
 
 			if (_v_control_mode.flag_control_attitude_enabled) {
 				control_attitude(dt);
@@ -797,8 +851,8 @@ MulticopterAttitudeControl::task_main()
 				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);
+				} else if (_v_rates_sp_virtual_pub > 0) {
+					_v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp);
 				}
 
 			} else {
@@ -819,11 +873,11 @@ MulticopterAttitudeControl::task_main()
 					_v_rates_sp.timestamp = hrt_absolute_time();
 
 					if (_v_rates_sp_pub > 0) {
-						orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+					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);
-					}
+				} else if (_v_rates_sp_virtual_pub > 0) {
+					_v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp);
+				}
 
 				} else {
 					/* attitude controller disabled, poll rates setpoint topic */
@@ -846,12 +900,13 @@ MulticopterAttitudeControl::task_main()
 				_actuators.timestamp = hrt_absolute_time();
 
 				if (!_actuators_0_circuit_breaker_enabled) {
-					if (_actuators_0_pub > 0) {
+					if (_actuators_0_pub > 0) {	//normal mutlicopter airframe
 						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
 
-					} else {
-						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+					} else if (_actuators_0_pub < 0 && _vehicle_status.is_rotary_wing) {
+						orb_publish(ORB_ID(actuator_controls_virtual_mc), _actuators_virtual_mc_pub, &_actuators);
 					}
+
 				}
 			}
 		}