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
2026-05-20 20:03:54 +08:00 · 2014-12-18 12:08:39 +01:00
parent 4402d7106b 6e874bed50
commit 16618f1ada
34 changed files with 1609 additions and 334 deletions
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -551,6 +551,45 @@ then
 		fi
 	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
 	#
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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);
@@ -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;

@@ -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 {
+				_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
+			}
+
+			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_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+				_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
 			}
-
-			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]);
-
-			} else {
-				/* advertise and publish */
-				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
-			}
-
 		}

 		loop_counter++;
@@ -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;
@@ -45,3 +45,5 @@ SRCS		= fw_pos_control_l1_main.cpp \
 		  mtecs/mTecs_params.c

 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
@@ -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;
@@ -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);
+				}
 			}
 		}
 	}
@@ -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 */
@@ -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();
 	}

@@ -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;
@@ -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;
@@ -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"),
@@ -466,6 +466,7 @@ public:
 		OCTA_X,
 		OCTA_PLUS,
 		OCTA_COX,
+		TWIN_ENGINE, /**< VTOL: one engine on each wing */

 		MAX_GEOMETRY
 	};
@@ -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;
@@ -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]]}

@@ -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);
@@ -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
@@ -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
@@ -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);
-ORB_DECLARE(actuator_controls_1);
-ORB_DECLARE(actuator_controls_2);
-ORB_DECLARE(actuator_controls_3);
 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_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 */
@@ -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
@@ -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);
+
@@ -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>
@@ -64,7 +64,6 @@ __EXPORT inline bool ok() { return !task_should_exit; }

 class Rate
 {
-
 public:
 	/**
 	 * Construct the Rate object and set rate