Merge remote-tracking branch 'upstream/master' into dev_ros

Conflicts:
	src/examples/subscriber/subscriber_params.c
	src/modules/mc_att_control/mc_att_control_main.cpp
	src/modules/uORB/topics/vehicle_attitude.h
	src/modules/uORB/topics/vehicle_attitude_setpoint.h
	src/platforms/px4_middleware.h

34 files changed, 1604 insertions, 329 deletions

diff --git a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance
new file mode 100644
index 000000000..2d5e272bd
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance
@@ -0,0 +1,31 @@
+#!nsh
+#
+# Team Blacksheep Discovery Long Range Quadcopter
+# 
+# Setup: 15 x 6.5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors
+#
+# Simon Wilks <simon@px4.io>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.07
+	param set MC_ROLLRATE_I 0.02
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_I 0.05
+	param set MC_PITCHRATE_D 0.003
+	param set MC_YAW_P 2.8
+	param set MC_YAWRATE_P 0.4
+	param set MC_YAWRATE_I 0.1
+	param set MC_YAWRATE_D 0.0
+fi
+
+set MIXER FMU_quad_w
+
+set PWM_OUTPUTS 1234
+set PWM_MIN 1200
diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol
new file mode 100644
index 000000000..bff971c0f
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol
@@ -0,0 +1,16 @@
+#!nsh
+#
+# Generic configuration file for caipirinha VTOL version
+#
+# Roman Bapst <bapstr@ethz.ch>
+#
+
+sh /etc/init.d/rc.vtol_defaults
+
+set MIXER FMU_caipirinha_vtol
+
+set PWM_OUTPUTS 12
+set PWM_MAX 2000
+set PWM_RATE 400
+param set VTOL_MOT_COUNT 2
+param set IDLE_PWM_MC  1080
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 496a52c5f..20f2be0d9 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -15,6 +15,7 @@
 # 10000	..	10999		Wide arm / H frame
 # 11000	..	11999		Hexa Cox
 # 12000	..	12999		Octo Cox
+# 13000 ..  13999		VTOL
 
 #
 # Simulation
@@ -220,6 +221,11 @@ then
 	sh /etc/init.d/10017_steadidrone_qu4d
 fi
 
+if param compare SYS_AUTOSTART 10018 18
+then
+	sh /etc/init.d/10018_tbs_endurance
+fi
+
 #
 # Hexa Coaxial
 #
@@ -237,3 +243,13 @@ if param compare SYS_AUTOSTART 12001
 then
 	sh /etc/init.d/12001_octo_cox
 fi
+
+# 13000 is historically reserved for the quadshot
+
+#
+# VTOL caipririnha
+#
+ if param compare SYS_AUTOSTART 13001
+ then
+	 sh /etc/init.d/13001_caipirinha_vtol
+ fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps
new file mode 100644
index 000000000..23ade6d78
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps
@@ -0,0 +1,15 @@
+#!nsh
+#
+# Standard apps for vtol:
+# att & pos estimator, att & pos control.
+#
+
+attitude_estimator_ekf start
+#ekf_att_pos_estimator start
+position_estimator_inav start
+
+vtol_att_control start
+mc_att_control start
+mc_pos_control start
+fw_att_control start
+fw_pos_control_l1 start
diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
new file mode 100644
index 000000000..10ee5db9b
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
@@ -0,0 +1,39 @@
+#!nsh
+
+set VEHICLE_TYPE vtol
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#
+	#Default controller parameters for MC
+	#
+	param set MC_ROLL_P 6.0
+	param set MC_ROLLRATE_P 0.1
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 6.0
+	param set MC_PITCHRATE_P 0.2
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.003
+	param set MC_YAW_P 4
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_D 0.0
+	param set MC_YAW_FF 0.3
+
+	#
+	# Default parameters for FW
+	#
+	param set FW_PR_FF 0.3
+	param set FW_PR_I 0.000
+	param set FW_PR_IMAX 0.2
+	param set FW_PR_P 0.02
+	param set FW_RR_FF 0.3
+	param set FW_RR_I 0.00
+	param set FW_RR_IMAX 0.2
+	param set FW_RR_P 0.02
+fi
+
+set PWM_DISARMED 900
+set PWM_MIN 1000
+set PWM_MAX 2000
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index b027107c8..5748fe2b5 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -552,6 +552,45 @@ then
 	fi
 
 	#
+	# VTOL setup
+	#
+	if [ $VEHICLE_TYPE == vtol ]
+	then
+		echo "[init] Vehicle type: VTOL"
+
+		if [ $MIXER == none ]
+		then
+			echo "Default mixer for vtol not defined"
+		fi
+
+		if [ $MAV_TYPE == none ]
+		then
+			# Use mixer to detect vehicle type
+			if [ $MIXER == FMU_caipirinha_vtol ]
+			then
+				set MAV_TYPE 19
+			fi
+		fi
+
+		# Still no MAV_TYPE found
+		if [ $MAV_TYPE == none ]
+		then
+			echo "Unknown MAV_TYPE"
+		else
+			param set MAV_TYPE $MAV_TYPE
+		fi
+
+		# Load mixer and configure outputs
+		sh /etc/init.d/rc.interface
+
+		# Start standard vtol apps
+		if [ $LOAD_DEFAULT_APPS == yes ]
+		then
+			sh /etc/init.d/rc.vtol_apps
+		fi
+	fi
+
+	#
 	# Start the navigator
 	#
 	navigator start
diff --git a/ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix b/ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix
new file mode 100644
index 000000000..5ae0f5588
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix
@@ -0,0 +1,16 @@
+#!nsh
+# Caipirinha vtol mixer for PX4FMU
+#
+#===========================
+R: 2- 10000 10000 10000 0
+
+#mixer for the elevons
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 0  10000  10000      0 -10000  10000
+S: 1 1  10000  10000      0 -10000  10000
+
+M: 2
+O:       10000   10000    0 -10000  10000
+S: 1 0   10000   10000    0 -10000  10000
+S: 1 1  -10000  -10000    0 -10000  10000
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index edf4fe1a0..ce12a2157 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -86,6 +86,7 @@ MODULES		+= modules/position_estimator_inav
 #MODULES		+= modules/fw_att_control
 MODULES		+= modules/mc_att_control
 MODULES		+= modules/mc_pos_control
+MODULES 	+= modules/vtol_att_control
 
 #
 # Logging
diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg
index 98018a1df..dbfd8b6bc 100644
--- a/msg/vehicle_attitude.msg
+++ b/msg/vehicle_attitude.msg
@@ -16,3 +16,20 @@ float32[4] q		# Quaternion (NED)
 float32[3] g_comp		# Compensated gravity vector
 bool R_valid		# Rotation matrix valid
 bool q_valid		# Quaternion valid
+
+# secondary attitude for VTOL
+float32 roll_sec		# Roll angle (rad, Tait-Bryan, NED)
+float32 pitch_sec		# Pitch angle (rad, Tait-Bryan, NED)
+float32 yaw_sec			# Yaw angle (rad, Tait-Bryan, NED)
+float32 rollspeed_sec		# Roll angular speed (rad/s, Tait-Bryan, NED)
+float32 pitchspeed_sec		# Pitch angular speed (rad/s, Tait-Bryan, NED)
+float32 yawspeed_sec	    	# Yaw angular speed (rad/s, Tait-Bryan, NED)
+float32 rollacc_sec		# Roll angular accelration (rad/s, Tait-Bryan, NED)
+float32 pitchacc_sec	    	# Pitch angular acceleration (rad/s, Tait-Bryan, NED)
+float32 yawacc_sec		# Yaw angular acceleration (rad/s, Tait-Bryan, NED)
+float32[3] rate_offsets_sec	# Offsets of the body angular rates from zero
+float32[9] R_sec		# Rotation matrix, body to world, (Tait-Bryan, NED)
+float32[4] q_sec		# Quaternion (NED)
+float32[3] g_comp_sec		# Compensated gravity vector
+bool R_valid_sec		# Rotation matrix valid
+bool q_valid_sec		# Quaternion valid
diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg
new file mode 100644
index 000000000..4d5d5ce09
--- /dev/null
+++ b/msg/vehicle_status.msg
@@ -0,0 +1,157 @@
+# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
+uint8 MAIN_STATE_MANUAL = 0
+uint8 MAIN_STATE_ALTCTL = 1
+uint8 MAIN_STATE_POSCTL = 2
+uint8 MAIN_STATE_AUTO_MISSION = 3
+uint8 MAIN_STATE_AUTO_LOITER = 4
+uint8 MAIN_STATE_AUTO_RTL = 5
+uint8 MAIN_STATE_ACRO = 6
+uint8 MAIN_STATE_OFFBOARD = 7
+uint8 MAIN_STATE_MAX = 8
+
+# If you change the order, add or remove arming_state_t states make sure to update the arrays
+# in state_machine_helper.cpp as well.
+uint8 ARMING_STATE_INIT = 0
+uint8 ARMING_STATE_STANDBY = 1
+uint8 ARMING_STATE_ARMED = 2
+uint8 ARMING_STATE_ARMED_ERROR = 3
+uint8 ARMING_STATE_STANDBY_ERROR = 4
+uint8 ARMING_STATE_REBOOT = 5
+uint8 ARMING_STATE_IN_AIR_RESTORE = 6
+uint8 ARMING_STATE_MAX = 7
+
+uint8 HIL_STATE_OFF = 0
+uint8 HIL_STATE_ON = 1
+
+# Navigation state, i.e. "what should vehicle do".
+uint8 NAVIGATION_STATE_MANUAL = 0		# Manual mode
+uint8 NAVIGATION_STATE_ALTCTL = 1		# Altitude control mode
+uint8 NAVIGATION_STATE_POSCTL = 2		# Position control mode
+uint8 NAVIGATION_STATE_AUTO_MISSION = 3		# Auto mission mode
+uint8 NAVIGATION_STATE_AUTO_LOITER = 4		# Auto loiter mode
+uint8 NAVIGATION_STATE_AUTO_RTL = 5		# Auto return to launch mode
+uint8 NAVIGATION_STATE_AUTO_RCRECOVER = 6	# RC recover mode
+uint8 NAVIGATION_STATE_AUTO_RTGS = 7		# Auto return to groundstation on data link loss
+uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 	# Auto land on engine failure
+uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9	# Auto land on gps failure (e.g. open loop loiter down)
+uint8 NAVIGATION_STATE_ACRO = 10		# Acro mode
+uint8 NAVIGATION_STATE_LAND = 11		# Land mode
+uint8 NAVIGATION_STATE_DESCEND = 12		# Descend mode (no position control)
+uint8 NAVIGATION_STATE_TERMINATION = 13		# Termination mode
+uint8 NAVIGATION_STATE_OFFBOARD = 14
+uint8 NAVIGATION_STATE_MAX = 15
+
+# VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol
+uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128
+uint8 VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64
+uint8 VEHICLE_MODE_FLAG_HIL_ENABLED = 32
+uint8 VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16
+uint8 VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8
+uint8 VEHICLE_MODE_FLAG_AUTO_ENABLED = 4
+uint8 VEHICLE_MODE_FLAG_TEST_ENABLED = 2
+uint8 VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
+
+# VEHICLE_TYPE, should match 1:1 MAVLink's MAV_TYPE ENUM
+uint8 VEHICLE_TYPE_GENERIC = 0		    # Generic micro air vehicle.
+uint8 VEHICLE_TYPE_FIXED_WING = 1	    # Fixed wing aircraft.
+uint8 VEHICLE_TYPE_QUADROTOR = 2	    # Quadrotor
+uint8 VEHICLE_TYPE_COAXIAL = 3		    # Coaxial helicopter
+uint8 VEHICLE_TYPE_HELICOPTER = 4	    # Normal helicopter with tail rotor.
+uint8 VEHICLE_TYPE_ANTENNA_TRACKER = 5	    # Ground installation
+uint8 VEHICLE_TYPE_GCS = 6		    # Operator control unit / ground control station
+uint8 VEHICLE_TYPE_AIRSHIP = 7		    # Airship, controlled
+uint8 VEHICLE_TYPE_FREE_BALLOON = 8	    # Free balloon, uncontrolled
+uint8 VEHICLE_TYPE_ROCKET = 9		    # Rocket
+uint8 VEHICLE_TYPE_GROUND_ROVER = 10	    # Ground rover
+uint8 VEHICLE_TYPE_SURFACE_BOAT = 11	    # Surface vessel, boat, ship
+uint8 VEHICLE_TYPE_SUBMARINE = 12	    # Submarine
+uint8 VEHICLE_TYPE_HEXAROTOR = 13	    # Hexarotor
+uint8 VEHICLE_TYPE_OCTOROTOR = 14	    # Octorotor
+uint8 VEHICLE_TYPE_TRICOPTER = 15	    # Octorotor
+uint8 VEHICLE_TYPE_FLAPPING_WING = 16	    # Flapping wing
+uint8 VEHICLE_TYPE_KITE = 17		    # Kite
+uint8 VEHICLE_TYPE_ONBOARD_CONTROLLER=18    # Onboard companion controller
+uint8 VEHICLE_TYPE_VTOL_DUOROTOR = 19	    # Vtol with two engines
+uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20	    # Vtol with four engines*/
+uint8 VEHICLE_TYPE_ENUM_END = 21
+
+uint8 VEHICLE_BATTERY_WARNING_NONE = 0	    # no battery low voltage warning active
+uint8 VEHICLE_BATTERY_WARNING_LOW = 1	    # warning of low voltage
+uint8 VEHICLE_BATTERY_WARNING_CRITICAL = 2  # alerting of critical voltage
+
+# state machine / state of vehicle.
+# Encodes the complete system state and is set by the commander app.
+uint16 counter   # incremented by the writing thread everytime new data is stored
+uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
+
+uint8 main_state		    	# main state machine
+uint8 nav_state				# set navigation state machine to specified value
+uint8 arming_state			# current arming state
+uint8 hil_state				# current hil state
+bool failsafe				# true if system is in failsafe state
+
+int32 system_type			# system type, inspired by MAVLink's VEHICLE_TYPE enum
+int32 system_id				# system id, inspired by MAVLink's system ID field
+int32 component_id			# subsystem / component id, inspired by MAVLink's component ID field
+
+bool is_rotary_wing
+
+bool condition_battery_voltage_valid
+bool condition_system_in_air_restore		# true if we can restore in mid air
+bool condition_system_sensors_initialized
+bool condition_system_returned_to_home
+bool condition_auto_mission_available
+bool condition_global_position_valid		# set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
+bool condition_launch_position_valid		# indicates a valid launch position
+bool condition_home_position_valid		# indicates a valid home position (a valid home position is not always a valid launch)
+bool condition_local_position_valid
+bool condition_local_altitude_valid
+bool condition_airspeed_valid			# set to true by the commander app if there is a valid airspeed measurement available
+bool condition_landed				# true if vehicle is landed, always true if disarmed
+bool condition_power_input_valid		# set if input power is valid
+float32 avionics_power_rail_voltage		# voltage of the avionics power rail
+
+bool rc_signal_found_once
+bool rc_signal_lost				# true if RC reception lost
+uint64 rc_signal_lost_timestamp			# Time at which the RC reception was lost
+bool rc_signal_lost_cmd				# true if RC lost mode is commanded
+bool rc_input_blocked				# set if RC input should be ignored
+
+bool data_link_lost				# datalink to GCS lost
+bool data_link_lost_cmd				# datalink to GCS lost mode commanded
+uint8 data_link_lost_counter			# counts unique data link lost events
+bool engine_failure				# Set to true if an engine failure is detected
+bool engine_failure_cmd				# Set to true if an engine failure mode is commanded
+bool gps_failure				# Set to true if a gps failure is detected
+bool gps_failure_cmd				# Set to true if a gps failure mode is commanded
+
+bool barometer_failure				# Set to true if a barometer failure is detected
+
+bool offboard_control_signal_found_once
+bool offboard_control_signal_lost
+bool offboard_control_signal_weak
+uint64 offboard_control_signal_lost_interval	# interval in microseconds without an offboard control message
+bool offboard_control_set_by_command		# true if the offboard mode was set by a mavlink command and should not be overridden by RC
+
+# see SYS_STATUS mavlink message for the following
+uint32 onboard_control_sensors_present
+uint32 onboard_control_sensors_enabled
+uint32 onboard_control_sensors_health
+
+float32 load					# processor load from 0 to 1
+float32 battery_voltage
+float32 battery_current
+float32 battery_remaining
+
+uint8 battery_warning    # current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum
+uint16 drop_rate_comm
+uint16 errors_comm
+uint16 errors_count1
+uint16 errors_count2
+uint16 errors_count3
+uint16 errors_count4
+
+bool circuit_breaker_engaged_power_check
+bool circuit_breaker_engaged_airspd_check
+bool circuit_breaker_engaged_enginefailure_check
+bool circuit_breaker_engaged_gpsfailure_check
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 9784ea1a1..4c64c88ae 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -571,6 +571,44 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
 					att.R_valid = true;
 
+					// compute secondary attitude
+					math::Matrix<3, 3> R_adapted;		//modified rotation matrix
+					R_adapted = 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_sec;		//adapted euler angles for fixed wing operation
+					euler_angles_sec = R_adapted.to_euler();
+
+					att.roll_sec    = euler_angles_sec(0);
+					att.pitch_sec   = euler_angles_sec(1);
+					att.yaw_sec     = euler_angles_sec(2);
+
+					memcpy(&att.R_sec[0], &R_adapted.data[0], sizeof(att.R_sec));
+
+					att.rollspeed_sec  = -x_aposteriori[2];
+					att.pitchspeed_sec = x_aposteriori[1];
+					att.yawspeed_sec   = x_aposteriori[0];
+					att.rollacc_sec    = -x_aposteriori[5];
+					att.pitchacc_sec   = x_aposteriori[4];
+					att.yawacc_sec     = x_aposteriori[3];
+
+					att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2));
+					att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1);
+					att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0);
+
+
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
 						// Broadcast
 						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 6d6b3a36c..744323c01 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -82,6 +82,7 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/vtol_vehicle_status.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -149,6 +150,9 @@ enum MAV_MODE_FLAG {
 /* Mavlink file descriptors */
 static int mavlink_fd = 0;
 
+/* Syste autostart ID */
+static int autostart_id;
+
 /* flags */
 static bool commander_initialized = false;
 static volatile bool thread_should_exit = false;		/**< daemon exit flag */
@@ -732,6 +736,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
 	param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
 	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
+	param_t _param_autostart_id = param_find("SYS_AUTOSTART");
 
 	/* welcome user */
 	warnx("starting");
@@ -1014,6 +1019,13 @@ int commander_thread_main(int argc, char *argv[])
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
 
+	/* Subscribe to vtol vehicle status topic */
+	int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
+	struct vtol_vehicle_status_s vtol_status;
+	memset(&vtol_status, 0, sizeof(vtol_status));
+	vtol_status.vtol_in_rw_mode = true;		//default for vtol is rotary wing
+
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -1070,7 +1082,10 @@ int commander_thread_main(int argc, char *argv[])
 				    status.system_type == VEHICLE_TYPE_TRICOPTER ||
 				    status.system_type == VEHICLE_TYPE_QUADROTOR ||
 				    status.system_type == VEHICLE_TYPE_HEXAROTOR ||
-				    status.system_type == VEHICLE_TYPE_OCTOROTOR) {
+				    status.system_type == VEHICLE_TYPE_OCTOROTOR ||
+				    (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
+				    (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
+
 					status.is_rotary_wing = true;
 
 				} else {
@@ -1106,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_ef_throttle_thres, &ef_throttle_thres);
 			param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
 			param_get(_param_ef_time_thres, &ef_time_thres);
+			param_get(_param_autostart_id, &autostart_id);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -1231,6 +1247,19 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/* update vtol vehicle status*/
+		orb_check(vtol_vehicle_status_sub, &updated);
+
+		if (updated) {
+			/* vtol status changed */
+			orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
+
+			/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
+			if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) {
+				status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
+			}
+		}
+
 		/* update global position estimate */
 		orb_check(global_position_sub, &updated);
 
@@ -2189,7 +2218,13 @@ set_control_mode()
 	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
 	/* TODO: check this */
-	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
+	if (autostart_id < 13000 || autostart_id >= 14000) {
+		control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
+
+	} else {
+		control_mode.flag_external_manual_override_ok = false;
+	}
+
 	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
 	control_mode.flag_control_offboard_enabled = false;
 
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 6bb9bdee3..53e854ae5 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>
  *
  */
 
@@ -93,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.
 	 */
@@ -113,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 */
@@ -131,11 +132,15 @@ private:
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
 	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_2_pub;		/**< actuator control group 1 setpoint (Airframe) */
+
+	orb_id_t _rates_sp_id;	// pointer to correct rates setpoint uORB metadata structure
+	orb_id_t _actuators_id;	// pointer to correct actuator controls0 uORB metadata structure
 
 	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 */
@@ -190,6 +195,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 {
@@ -229,6 +236,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 */
 
 
@@ -290,7 +299,7 @@ private:
 	static void	task_main_trampoline(int argc, char *argv[]);
 
 	/**
-	 * Main sensor collection task.
+	 * Main attitude controller collection task.
 	 */
 	void		task_main();
 
@@ -328,7 +337,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_rate_sp_pub(-1),
 	_attitude_sp_pub(-1),
 	_actuators_0_pub(-1),
-	_actuators_1_pub(-1),
+	_actuators_2_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -342,6 +351,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_att = {};
 	_accel = {};
 	_att_sp = {};
+	_rates_sp = {};
 	_manual = {};
 	_airspeed = {};
 	_vcontrol_mode = {};
@@ -387,8 +397,19 @@ 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();
+	// set correct uORB ID, depending on if vehicle is VTOL or not
+	if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/
+		_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
+		_actuators_id = ORB_ID(actuator_controls_virtual_fw);
+	}
+	else {
+		_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+		_actuators_id = ORB_ID(actuator_controls_0);
+	}
 }
 
 FixedwingAttitudeControl::~FixedwingAttitudeControl()
@@ -463,6 +484,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);
@@ -498,7 +520,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) {
@@ -530,7 +552,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);
 	}
 }
 
@@ -680,6 +701,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();
@@ -697,7 +777,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 {
@@ -706,10 +786,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;");
 			}
 
@@ -821,18 +901,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();
@@ -934,20 +1014,18 @@ 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(_rates_sp_id, _rate_sp_pub, &_rates_sp);
 				} else {
-					/* advertise and publish */
-					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+					/* advertise the attitude rates setpoint */
+					_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
 				}
 
 			} else {
@@ -967,28 +1045,21 @@ FixedwingAttitudeControl::task_main()
 			_actuators.timestamp = hrt_absolute_time();
 			_actuators_airframe.timestamp = hrt_absolute_time();
 
+			/* publish the actuator controls */
 			if (_actuators_0_pub > 0) {
-				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
-
+				orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
 			} else {
-				/* advertise and publish */
-				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+				_actuators_0_pub= orb_advertise(_actuators_id, &_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/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 4c969a5ca..3668f9211 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1401,8 +1401,6 @@ FixedwingPositionControl::task_main()
 			continue;
 		}
 
-		perf_begin(_loop_perf);
-
 		/* check vehicle control mode for changes to publication state */
 		vehicle_control_mode_poll();
 
@@ -1421,6 +1419,7 @@ FixedwingPositionControl::task_main()
 
 		/* only run controller if position changed */
 		if (fds[1].revents & POLLIN) {
+			perf_begin(_loop_perf);
 
 			/* XXX Hack to get mavlink output going */
 			if (_mavlink_fd < 0) {
@@ -1475,10 +1474,9 @@ FixedwingPositionControl::task_main()
 				}
 
 			}
-
+			perf_end(_loop_perf);
 		}
 
-		perf_end(_loop_perf);
 	}
 
 	_task_running = false;
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
index 15b575b50..440bab2c5 100644
--- a/src/modules/fw_pos_control_l1/module.mk
+++ b/src/modules/fw_pos_control_l1/module.mk
@@ -45,3 +45,5 @@ SRCS		= fw_pos_control_l1_main.cpp \
 		  mtecs/mTecs_params.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 5908279d5..378e3427d 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1351,7 +1351,10 @@ protected:
 			/* scale outputs depending on system type */
 			if (system_type == MAV_TYPE_QUADROTOR ||
 				system_type == MAV_TYPE_HEXAROTOR ||
-				system_type == MAV_TYPE_OCTOROTOR) {
+				system_type == MAV_TYPE_OCTOROTOR ||
+				system_type == MAV_TYPE_VTOL_DUOROTOR ||
+				system_type == MAV_TYPE_VTOL_QUADROTOR) {
+
 				/* multirotors: set number of rotor outputs depending on type */
 
 				unsigned n;
@@ -1365,6 +1368,14 @@ protected:
 					n = 6;
 					break;
 
+				case MAV_TYPE_VTOL_DUOROTOR:
+					n = 2;
+					break;
+
+				case MAV_TYPE_VTOL_QUADROTOR:
+					n = 4;
+					break;
+
 				default:
 					n = 8;
 					break;
diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 89da8438b..90a6c90fe 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -97,9 +97,17 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.acro_roll_max		= 	PX4_PARAM_INIT(MC_ACRO_R_MAX);
 	_params_handles.acro_pitch_max		= 	PX4_PARAM_INIT(MC_ACRO_P_MAX);
 	_params_handles.acro_yaw_max		= 	PX4_PARAM_INIT(MC_ACRO_Y_MAX);
+	_params_handles.autostart_id		= 	PX4_PARAM_INIT(SYS_AUTOSTART);
 
 	/* fetch initial parameter values */
 	parameters_update();
+	/* set correct uORB ID, depending on if vehicle is VTOL or not */
+	if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/
+		_is_vtol = true;
+	}
+	else {
+		_is_vtol = false;
+	}
 
 	/*
 	 * do subscriptions
@@ -108,9 +116,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_v_att_sp = PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
 	_v_rates_sp = PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
 	_v_control_mode = PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
-	PX4_SUBSCRIBE(_n, parameter_update, MulticopterAttitudeControl::handle_parameter_update, this, 1000);
+	_parameter_update = PX4_SUBSCRIBE(_n, parameter_update,
+			MulticopterAttitudeControl::handle_parameter_update, this, 1000);
 	_manual_control_sp = PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
 	_armed = PX4_SUBSCRIBE(_n, actuator_armed, 0);
+	_v_status = PX4_SUBSCRIBE(_n, vehicle_status, 0);
 
 }
 
@@ -204,7 +214,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		control_attitude(dt);
 
 		/* publish the attitude setpoint if needed */
-		if (_publish_att_sp) {
+		if (_publish_att_sp && _v_status->get().is_rotary_wing) {
 			_v_att_sp_mod.timestamp = px4::get_time_micros();
 
 			if (_att_sp_pub != nullptr) {
@@ -225,7 +235,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		if (_v_rates_sp_pub != nullptr) {
 			_v_rates_sp_pub->publish(_v_rates_sp_mod);
 		} else {
-			_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
+			if (_is_vtol) {
+				_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
+			} else {
+				_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
+			}
 		}
 
 	} else {
@@ -250,7 +264,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 				_v_rates_sp_pub->publish(_v_rates_sp_mod);
 
 			} else {
-				_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
+				if (_is_vtol) {
+					_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
+				} else {
+					_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
+				}
 			}
 
 		} else {
@@ -277,7 +295,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 				_actuators_0_pub->publish(_actuators);
 
 			} else {
-				_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);
+				if (_is_vtol) {
+					_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc);
+				} else {
+					_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);
+				}
 			}
 		}
 	}
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
index 76c095cc1..33552c269 100644
--- a/src/modules/mc_att_control/mc_att_control.h
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -93,6 +93,8 @@ private:
 	px4::Publisher *	_v_rates_sp_pub;		/**< rate setpoint publication */
 	px4::Publisher *	_actuators_0_pub;		/**< attitude actuator controls publication */
 
+	bool _is_vtol;						/**< true if vehicle is vtol, to be replaced with global API */
+
 	px4::NodeHandle _n;
 
 	struct {
@@ -117,6 +119,8 @@ private:
 		px4_param_t acro_roll_max;
 		px4_param_t acro_pitch_max;
 		px4_param_t acro_yaw_max;
+
+		px4_param_t autostart_id;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	perf_counter_t _loop_perf; /**< loop performance counter */
diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index 871f93ab8..d7fbf3acd 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -266,7 +266,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
 {
 	/* reset integral if disarmed */
-	if (!_armed->get().armed) {
+	if (!_armed->get().armed || !_v_status->get().is_rotary_wing) {
 		_rates_int.zero();
 	}
 
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 17c779796..cf4c726a7 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -87,11 +87,13 @@ public:
 
 protected:
 	px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att;			    /**< vehicle attitude */
-	px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp;  /**< manual control setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp;	    /**< vehicle attitude setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp;	    /**< vehicle rates setpoint */
 	px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode;	    /**< vehicle control mode */
+	px4::PX4_SUBSCRIBER(parameter_update) *_parameter_update;	    /**< parameter update */
+	px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp;   /**< manual control setpoint */
 	px4::PX4_SUBSCRIBER(actuator_armed) *_armed;			    /**< actuator arming status */
-	px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp;	/**< vehicle attitude setpoint */
-	px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp;	/**< vehicle rates setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_status) *_v_status;			    /**< vehicle status */
 
 	PX4_TOPIC_T(vehicle_attitude_setpoint)	_v_att_sp_mod;	/**< modified vehicle attitude setpoint
 								  that gets published eventually */
@@ -121,6 +123,8 @@ protected:
 		float man_pitch_max;
 		float man_yaw_max;
 		math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
+
+		int32_t autostart_id;
 	} _params;
 
 	bool _publish_att_sp;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 0b6861d2a..6df677338 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -941,6 +941,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_rates_setpoint_s rates_sp;
 		struct actuator_outputs_s act_outputs;
 		struct actuator_controls_s act_controls;
+		struct actuator_controls_s act_controls1;
 		struct vehicle_local_position_s local_pos;
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
@@ -1022,6 +1023,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int rates_sp_sub;
 		int act_outputs_sub;
 		int act_controls_sub;
+		int act_controls_1_sub;
 		int local_pos_sub;
 		int local_pos_sp_sub;
 		int global_pos_sub;
@@ -1055,6 +1057,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 	subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
 	subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1));
 	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -1375,6 +1378,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_ATT.gy = buf.att.g_comp[1];
 			log_msg.body.log_ATT.gz = buf.att.g_comp[2];
 			LOGBUFFER_WRITE_AND_COUNT(ATT);
+			// secondary attitude
+			log_msg.msg_type = LOG_ATT2_MSG;
+			log_msg.body.log_ATT.roll = buf.att.roll_sec;
+			log_msg.body.log_ATT.pitch = buf.att.pitch_sec;
+			log_msg.body.log_ATT.yaw = buf.att.yaw_sec;
+			log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec;
+			log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec;
+			log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec;
+			log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0];
+			log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1];
+			log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2];
+			LOGBUFFER_WRITE_AND_COUNT(ATT);
 		}
 
 		/* --- ATTITUDE SETPOINT --- */
@@ -1413,6 +1428,16 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(ATTC);
 		}
 
+		/* --- ACTUATOR CONTROL FW VTOL --- */
+		if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) {
+			log_msg.msg_type = LOG_ATC1_MSG;
+			log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
+			log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
+			log_msg.body.log_ATTC.yaw = buf.act_controls.control[2];
+			log_msg.body.log_ATTC.thrust = buf.act_controls.control[3];
+			LOGBUFFER_WRITE_AND_COUNT(ATTC);
+		}
+
 		/* --- LOCAL POSITION --- */
 		if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
 			log_msg.msg_type = LOG_LPOS_MSG;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 30491036a..b78b430aa 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -50,6 +50,7 @@
 #pragma pack(push, 1)
 /* --- ATT - ATTITUDE --- */
 #define LOG_ATT_MSG 2
+#define LOG_ATT2_MSG 41
 struct log_ATT_s {
 	float roll;
 	float pitch;
@@ -149,6 +150,7 @@ struct log_GPS_s {
 
 /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
 #define LOG_ATTC_MSG 9
+#define LOG_ATC1_MSG 40
 struct log_ATTC_s {
 	float roll;
 	float pitch;
@@ -422,7 +424,6 @@ struct log_ENCD_s {
 	float vel1;
 };
 
-
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -450,7 +451,8 @@ struct log_PARM_s {
 /* construct list of all message formats */
 static const struct log_format_s log_formats[] = {
 	/* business-level messages, ID < 0x80 */
-	LOG_FORMAT(ATT, "fffffffff",		"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
+	LOG_FORMAT_S(ATT, ATT, "fffffffff",		"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
+	LOG_FORMAT_S(ATT2, ATT, "fffffffff",	"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
 	LOG_FORMAT(ATSP, "ffff",		"RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT_S(IMU, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT_S(IMU1, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
@@ -459,7 +461,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(LPOS, "ffffffffLLfBBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
 	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
-	LOG_FORMAT(ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
+	LOG_FORMAT_S(ATTC, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
+	LOG_FORMAT_S(ATC1, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBBfBB",		"MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffBB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
 	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 17989558e..1fe4380ad 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -466,6 +466,7 @@ public:
 		OCTA_X,
 		OCTA_PLUS,
 		OCTA_COX,
+		TWIN_ENGINE, /**< VTOL: one engine on each wing */
 
 		MAX_GEOMETRY
 	};
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 57e17b67d..24187c9bc 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -76,6 +76,7 @@ float constrain(float val, float min, float max)
 /*
  * These tables automatically generated by multi_tables - do not edit.
  */
+
 const MultirotorMixer::Rotor _config_quad_x[] = {
 	{ -0.707107,  0.707107,  1.00 },
 	{  0.707107, -0.707107,  1.00 },
@@ -88,11 +89,12 @@ const MultirotorMixer::Rotor _config_quad_plus[] = {
 	{  0.000000,  1.000000, -1.00 },
 	{ -0.000000, -1.000000, -1.00 },
 };
+//Add table for quad in V configuration, which is not generated by multi_tables!
 const MultirotorMixer::Rotor _config_quad_v[] = {
-	{ -0.927184,  0.374607,  1.00 },
-	{  0.694658, -0.719340,  1.00 },
-	{  0.927184,  0.374607, -1.00 },
-	{ -0.694658, -0.719340, -1.00 },
+	{ -0.3223,  0.9466,   0.4242 },
+	{  0.3223, -0.9466,   1.0000 },
+	{  0.3223,  0.9466,  -0.4242 },
+	{ -0.3223, -0.9466,  -1.0000 },
 };
 const MultirotorMixer::Rotor _config_quad_wide[] = {
 	{ -0.927184,  0.374607,  1.00 },
@@ -154,6 +156,11 @@ const MultirotorMixer::Rotor _config_octa_cox[] = {
 	{ -0.707107, -0.707107,  1.00 },
 	{  0.707107, -0.707107, -1.00 },
 };
+const MultirotorMixer::Rotor _config_duorotor[] = {
+	{ -1.000000,  0.000000,  0.00 },
+	{  1.000000,  0.000000,  0.00 },
+};
+
 const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_quad_x[0],
 	&_config_quad_plus[0],
@@ -165,6 +172,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_octa_x[0],
 	&_config_octa_plus[0],
 	&_config_octa_cox[0],
+	&_config_duorotor[0],
 };
 const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	4, /* quad_x */
@@ -177,6 +185,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	8, /* octa_x */
 	8, /* octa_plus */
 	8, /* octa_cox */
+	2, /* twin_engine */
 };
 
 }
@@ -274,6 +283,8 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	} else if (!strcmp(geomname, "8c")) {
 		geometry = MultirotorMixer::OCTA_COX;
 
+	} else if (!strcmp(geomname, "2-")) {
+		geometry = MultirotorMixer::TWIN_ENGINE;
 	} else {
 		debug("unrecognised geometry '%s'", geomname);
 		return nullptr;
diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables
index b5698036e..18c828578 100755
--- a/src/modules/systemlib/mixer/multi_tables
+++ b/src/modules/systemlib/mixer/multi_tables
@@ -6,6 +6,7 @@
 proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) }
 proc rcos {a} { expr cos([rad $a])}
 
+
 set quad_x {
 	  45 CCW
 	-135 CCW
@@ -20,12 +21,6 @@ set quad_plus {
 	 180 CW
 }
 
-set quad_v {
-	  68 CCW
-	-136 CCW
-	 -68 CW
-	 136 CW
-}
 
 set quad_wide {
 	  68 CCW
@@ -94,7 +89,9 @@ set octa_cox {
 	-135 CW
 }
 
-set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
+
+set tables {quad_x quad_plus quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
+
 
 proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
 
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 49dfc7834..1141431cc 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -91,6 +91,9 @@ ORB_DEFINE(home_position, struct home_position_s);
 #include "topics/vehicle_status.h"
 ORB_DEFINE(vehicle_status, struct vehicle_status_s);
 
+#include "topics/vtol_vehicle_status.h"
+ORB_DEFINE(vtol_vehicle_status, struct vtol_vehicle_status_s);
+
 #include "topics/safety.h"
 ORB_DEFINE(safety, struct safety_s);
 
@@ -114,6 +117,8 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
 
 #include "topics/vehicle_rates_setpoint.h"
 ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
+ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
+ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
 
 #include "topics/rc_channels.h"
 ORB_DEFINE(rc_channels, struct rc_channels_s);
@@ -148,6 +153,8 @@ ORB_DEFINE(fence, unsigned);
 
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
+ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
+ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
 
 #include "topics/manual_control_setpoint.h"
 ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
@@ -182,6 +189,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
+//Virtual control groups, used for VTOL operation
+ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s);
+ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s);
 
 #include "topics/actuator_armed.h"
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
deleted file mode 100644
index 131029061..000000000
--- a/src/modules/uORB/topics/vehicle_status.h
+++ /dev/null
@@ -1,256 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 - 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 vehicle_status.h
- * Definition of the vehicle_status uORB topic.
- *
- * Published the state machine and the system status bitfields
- * (see SYS_STATUS mavlink message), published only by commander app.
- *
- * All apps should write to subsystem_info:
- *
- *  (any app) --> subsystem_info (published) --> (commander app state machine)  --> vehicle_status --> (mavlink app)
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <julian@oes.ch>
- */
-
-#ifndef VEHICLE_STATUS_H_
-#define VEHICLE_STATUS_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <platforms/px4_defines.h>
-
-/**
- * @addtogroup topics @{
- */
-
-/**
- * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
- */
-typedef enum {
-	MAIN_STATE_MANUAL = 0,
-	MAIN_STATE_ALTCTL,
-	MAIN_STATE_POSCTL,
-	MAIN_STATE_AUTO_MISSION,
-	MAIN_STATE_AUTO_LOITER,
-	MAIN_STATE_AUTO_RTL,
-	MAIN_STATE_ACRO,
-	MAIN_STATE_OFFBOARD,
-	MAIN_STATE_MAX
-} main_state_t;
-
-// If you change the order, add or remove arming_state_t states make sure to update the arrays
-// in state_machine_helper.cpp as well.
-typedef enum {
-	ARMING_STATE_INIT = 0,
-	ARMING_STATE_STANDBY,
-	ARMING_STATE_ARMED,
-	ARMING_STATE_ARMED_ERROR,
-	ARMING_STATE_STANDBY_ERROR,
-	ARMING_STATE_REBOOT,
-	ARMING_STATE_IN_AIR_RESTORE,
-	ARMING_STATE_MAX,
-} arming_state_t;
-
-typedef enum {
-	HIL_STATE_OFF = 0,
-	HIL_STATE_ON
-} hil_state_t;
-
-/**
- * Navigation state, i.e. "what should vehicle do".
- */
-typedef enum {
-	NAVIGATION_STATE_MANUAL = 0,		/**< Manual mode */
-	NAVIGATION_STATE_ALTCTL,		/**< Altitude control mode */
-	NAVIGATION_STATE_POSCTL,		/**< Position control mode */
-	NAVIGATION_STATE_AUTO_MISSION,		/**< Auto mission mode */
-	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
-	NAVIGATION_STATE_AUTO_RTL,		/**< Auto return to launch mode */
-	NAVIGATION_STATE_AUTO_RCRECOVER,	/**< RC recover mode */
-	NAVIGATION_STATE_AUTO_RTGS,		/**< Auto return to groundstation on data link loss */
-	NAVIGATION_STATE_AUTO_LANDENGFAIL,	/**< Auto land on engine failure */
-	NAVIGATION_STATE_AUTO_LANDGPSFAIL,	/**< Auto land on gps failure (e.g. open loop loiter down) */
-	NAVIGATION_STATE_ACRO,			/**< Acro mode */
-	NAVIGATION_STATE_LAND,			/**< Land mode */
-	NAVIGATION_STATE_DESCEND,		/**< Descend mode (no position control) */
-	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
-	NAVIGATION_STATE_OFFBOARD,
-	NAVIGATION_STATE_MAX,
-} navigation_state_t;
-
-enum VEHICLE_MODE_FLAG {
-	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
-	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
-	VEHICLE_MODE_FLAG_HIL_ENABLED = 32,
-	VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16,
-	VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8,
-	VEHICLE_MODE_FLAG_AUTO_ENABLED = 4,
-	VEHICLE_MODE_FLAG_TEST_ENABLED = 2,
-	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
-}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
-
-/**
- * Should match 1:1 MAVLink's MAV_TYPE ENUM
- */
-enum VEHICLE_TYPE {
-	VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */
-	VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */
-	VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */
-	VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */
-	VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */
-	VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */
-	VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */
-	VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */
-	VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */
-	VEHICLE_TYPE_ROCKET = 9, /* Rocket | */
-	VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */
-	VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */
-	VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */
-	VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */
-	VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */
-	VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */
-	VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */
-	VEHICLE_TYPE_KITE = 17, /* Kite | */
-	VEHICLE_TYPE_ENUM_END = 18, /*  | */
-};
-
-enum VEHICLE_BATTERY_WARNING {
-	VEHICLE_BATTERY_WARNING_NONE = 0,	/**< no battery low voltage warning active */
-	VEHICLE_BATTERY_WARNING_LOW,	/**< warning of low voltage */
-	VEHICLE_BATTERY_WARNING_CRITICAL	/**< alerting of critical voltage */
-};
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * state machine / state of vehicle.
- *
- * Encodes the complete system state and is set by the commander app.
- */
-struct vehicle_status_s {
-	/* use of a counter and timestamp recommended (but not necessary) */
-
-	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
-	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
-
-	main_state_t main_state;		    	/**< main state machine */
-	navigation_state_t nav_state;		/**< set navigation state machine to specified value */
-	arming_state_t arming_state;			/**< current arming state */
-	hil_state_t hil_state;				/**< current hil state */
-	bool failsafe;					/**< true if system is in failsafe state */
-
-	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
-	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
-	int32_t component_id;				/**< subsystem / component id, inspired by MAVLink's component ID field */
-
-	bool is_rotary_wing;
-
-	bool condition_battery_voltage_valid;
-	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */
-	bool condition_system_sensors_initialized;
-	bool condition_system_returned_to_home;
-	bool condition_auto_mission_available;
-	bool condition_global_position_valid;		/**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */
-	bool condition_launch_position_valid;		/**< indicates a valid launch position */
-	bool condition_home_position_valid;		/**< indicates a valid home position (a valid home position is not always a valid launch) */
-	bool condition_local_position_valid;
-	bool condition_local_altitude_valid;
-	bool condition_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
-	bool condition_landed;					/**< true if vehicle is landed, always true if disarmed */
-	bool condition_power_input_valid;		/**< set if input power is valid */
-	float avionics_power_rail_voltage;		/**< voltage of the avionics power rail */
-
-	bool rc_signal_found_once;
-	bool rc_signal_lost;				/**< true if RC reception lost */
-	uint64_t rc_signal_lost_timestamp;		/**< Time at which the RC reception was lost */
-	bool rc_signal_lost_cmd;				/**< true if RC lost mode is commanded */
-	bool rc_input_blocked;				/**< set if RC input should be ignored */
-
-	bool data_link_lost;				/**< datalink to GCS lost */
-	bool data_link_lost_cmd;			/**< datalink to GCS lost mode commanded */
-	uint8_t data_link_lost_counter;			/**< counts unique data link lost events */
-	bool engine_failure;				/** Set to true if an engine failure is detected */
-	bool engine_failure_cmd;			/** Set to true if an engine failure mode is commanded */
-	bool gps_failure;				/** Set to true if a gps failure is detected */
-	bool gps_failure_cmd;				/** Set to true if a gps failure mode is commanded */
-
-	bool barometer_failure;				/** Set to true if a barometer failure is detected */
-
-	bool offboard_control_signal_found_once;
-	bool offboard_control_signal_lost;
-	bool offboard_control_signal_weak;
-	uint64_t offboard_control_signal_lost_interval;	/**< interval in microseconds without an offboard control message */
-	bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command
-						    and should not be overridden by RC */
-
-	/* see SYS_STATUS mavlink message for the following */
-	uint32_t onboard_control_sensors_present;
-	uint32_t onboard_control_sensors_enabled;
-	uint32_t onboard_control_sensors_health;
-
-	float load;					/**< processor load from 0 to 1 */
-	float battery_voltage;
-	float battery_current;
-	float battery_remaining;
-
-	enum VEHICLE_BATTERY_WARNING battery_warning;    /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
-	uint16_t drop_rate_comm;
-	uint16_t errors_comm;
-	uint16_t errors_count1;
-	uint16_t errors_count2;
-	uint16_t errors_count3;
-	uint16_t errors_count4;
-
-	bool circuit_breaker_engaged_power_check;
-	bool circuit_breaker_engaged_airspd_check;
-	bool circuit_breaker_engaged_enginefailure_check;
-	bool circuit_breaker_engaged_gpsfailure_check;
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_status);
-
-#endif
diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h
new file mode 100644
index 000000000..24ecca9fa
--- /dev/null
+++ b/src/modules/uORB/topics/vtol_vehicle_status.h
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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_status.h
+ *
+ * Vtol status topic
+ *
+ */
+
+#ifndef TOPIC_VTOL_STATUS_H
+#define TOPIC_VTOL_STATUS_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/* Indicates in which mode the vtol aircraft is in */
+struct vtol_vehicle_status_s {
+
+	uint64_t	timestamp;	/**< Microseconds since system boot */
+	bool vtol_in_rw_mode;	/*true: vtol vehicle is in rotating wing mode */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vtol_vehicle_status);
+
+#endif
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 0a99cce77..a6e9171ed 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -264,9 +264,28 @@ __END_DECLS
 /* Diverse uORB header defines */ //XXX: move to better location
 #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS    ORB_ID(actuator_controls_0)
 ORB_DECLARE(actuator_controls_0);
+typedef struct actuator_controls_s actuator_controls_0_s;
 ORB_DECLARE(actuator_controls_1);
+typedef struct actuator_controls_s actuator_controls_1_s;
 ORB_DECLARE(actuator_controls_2);
+typedef struct actuator_controls_s actuator_controls_2_s;
 ORB_DECLARE(actuator_controls_3);
-typedef struct actuator_controls_s actuator_controls_0_s;
+typedef struct actuator_controls_s actuator_controls_3_s;
+ORB_DECLARE(actuator_controls_virtual_mc);
+typedef struct actuator_controls_s actuator_controls_virtual_mc_s;
+ORB_DECLARE(actuator_controls_virtual_fw);
+typedef struct actuator_controls_s actuator_controls_virtual_fw_s;
+ORB_DECLARE(mc_virtual_rates_setpoint);
+typedef struct vehicle_rates_setpoint_s mc_virtual_rates_setpoint_s;
+ORB_DECLARE(fw_virtual_rates_setpoint);
+typedef struct vehicle_rates_setpoint_s fw_virtual_rates_setpoint_s;
+ORB_DECLARE(mc_virtual_attitude_setpoint);
+typedef struct vehicle_attitude_setpoint_s mc_virtual_attitude_setpoint_s;
+ORB_DECLARE(fw_virtual_attitude_setpoint);
+typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s;
+typedef uint8_t arming_state_t;
+typedef uint8_t main_state_t;
+typedef uint8_t hil_state_t;
+typedef uint8_t navigation_state_t;
 
 #endif /* _UORB_UORB_H */
diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk
new file mode 100644
index 000000000..0d5155e90
--- /dev/null
+++ b/src/modules/vtol_att_control/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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 \
+			  vtol_att_control_params.c
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..24800cce8
--- /dev/null
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -0,0 +1,812 @@
+/****************************************************************************
+ *
+ *   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/airspeed.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 	_airspeed_sub;			// airspeed 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 airspeed_s 					_airspeed;			// airspeed
+
+	struct {
+		param_t idle_pwm_mc;	//pwm value for idle in mc mode
+		param_t vtol_motor_count;
+		float mc_airspeed_min;		// min airspeed in multicoper mode (including prop-wash)
+		float mc_airspeed_trim;		// trim airspeed in multicopter mode
+		float mc_airspeed_max;		// max airpseed in multicopter mode
+	} _params;
+
+	struct {
+		param_t idle_pwm_mc;
+		param_t vtol_motor_count;
+		param_t mc_airspeed_min;
+		param_t mc_airspeed_trim;
+		param_t mc_airspeed_max;
+	} _params_handles;
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
+
+	/* 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"
+	unsigned _motor_count;	// number of motors
+
+//*****************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 		vehicle_airspeed_poll();		// Check for changes in airspeed
+	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();
+	void 		scale_mc_output();
+};
+
+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),
+	_airspeed_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, "vtol_att_control")),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input"))
+{
+
+	flag_idle_mc = true;
+
+	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));
+	memset(&_airspeed,0,sizeof(_airspeed));
+
+	_params.idle_pwm_mc = PWM_LOWEST_MIN;
+	_params.vtol_motor_count = 0;
+
+	_params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC");
+	_params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT");
+	_params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN");
+	_params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX");
+	_params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM");
+
+	/* 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 airspeed updates.
+*/
+void
+VtolAttitudeControl::vehicle_airspeed_poll() {
+	bool updated;
+	orb_check(_airspeed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(airspeed), _airspeed_sub , &_airspeed);
+	}
+}
+
+/**
+* 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()
+{
+	float v;
+	/* idle pwm for mc mode */
+	param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
+
+	/* vtol motor count */
+	param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count);
+
+	/* vtol mc mode min airspeed */
+	param_get(_params_handles.mc_airspeed_min, &v);
+	_params.mc_airspeed_min = v;
+
+	/* vtol mc mode max airspeed */
+	param_get(_params_handles.mc_airspeed_max, &v);
+	_params.mc_airspeed_max = v;
+
+	/* vtol mc mode trim airspeed */
+	param_get(_params_handles.mc_airspeed_trim, &v);
+	_params.mc_airspeed_trim = 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
+	// unused now but still logged
+	_actuators_out_1.control[2] = _actuators_fw_in.control[2];	// yaw
+	_actuators_out_1.control[3] = _actuators_fw_in.control[3];	// throttle
+}
+
+/**
+* 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 < _params.vtol_motor_count; 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 = _params.idle_pwm_mc;
+	struct pwm_output_values pwm_values;
+	memset(&pwm_values, 0, sizeof(pwm_values));
+
+	for (unsigned i = 0; i < _params.vtol_motor_count; 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::scale_mc_output() {
+	// 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 = _params.mc_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);
+		}
+
+	// Sanity check if airspeed is consistent with throttle
+	if(_manual_control_sp.z >= 0.35f && airspeed < _params.mc_airspeed_trim) {	// XXX magic number, should be hover throttle param
+		airspeed = _params.mc_airspeed_trim;
+	}
+
+	/*
+	 * 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 = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed);
+	_actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
+}
+
+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));
+	_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));
+
+	_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
+
+	// make sure we start with idle in mc mode
+	set_idle_mc();
+	flag_idle_mc = true;
+
+	/* 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();
+		vehicle_airspeed_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);
+				// scale pitch control with airspeed
+				scale_mc_output();
+
+				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;
+}
diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
new file mode 100644
index 000000000..44157acba
--- /dev/null
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -0,0 +1,13 @@
+#include <systemlib/param/param.h>
+
+// number of engines
+PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0);
+// idle pwm in multicopter mode
+PARAM_DEFINE_INT32(IDLE_PWM_MC,900);
+// min airspeed in multicopter mode
+PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2);
+// max airspeed in multicopter mode
+PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30);
+// trim airspeed in multicopter mode
+PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10);
+
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 752c6b5fe..6c36b692a 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -59,6 +59,7 @@
 #include <px4/vehicle_control_mode.h>
 #include <px4/actuator_armed.h>
 #include <px4/parameter_update.h>
+#include <px4/parameter_status.h>
 #endif
 
 #else
@@ -76,6 +77,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index 16e73ec04..a8f3ad666 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -64,7 +64,6 @@ __EXPORT inline bool ok() { return !task_should_exit; }
 
 class Rate
 {
-
 public:
 	/**
 	 * Construct the Rate object and set rate