Merge pull request #2652 from PX4/vtol_cleanup

WIP: Vtol cleanup

ROMFS/px4fmu_common/init.d/13002_firefly6

@@ -46,5 +46,6 @@ set PWM_AUX_MAX 2000
 set MAV_TYPE 21
 
 param set VT_MOT_COUNT 6
+param set VT_FW_MOT_OFF 23
 param set VT_IDLE_PWM_MC 1080
 param set VT_TYPE 1

msg/vehicle_status.msg

@@ -106,6 +106,7 @@ uint32 component_id			# subsystem / component id, inspired by MAVLink's componen
 bool is_rotary_wing			# True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
 bool is_vtol				# True if the system is VTOL capable
 bool vtol_fw_permanent_stab		# True if vtol should stabilize attitude for fw in manual mode
+bool in_transition_mode
 
 bool condition_battery_voltage_valid
 bool condition_system_in_air_restore		# true if we can restore in mid air

msg/vtol_vehicle_status.msg

@@ -1,4 +1,5 @@
 uint64 timestamp		# Microseconds since system boot
 bool vtol_in_rw_mode		# true: vtol vehicle is in rotating wing mode
+bool vtol_in_trans_mode
 bool fw_permanent_stab		# In fw mode stabilize attitude even if in manual mode
 float32 airspeed_tot		# Estimated airspeed over control surfaces

src/modules/commander/commander.cpp

@@ -197,6 +197,8 @@ static struct home_position_s _home;
 
 static unsigned _last_mission_instance = 0;
 
+struct vtol_vehicle_status_s vtol_status;
+
 /**
  * The daemon app only briefly exists to start
  * the background job. The stack size assigned in the
@@ -1169,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* 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;
+	//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
 
@@ -1496,6 +1498,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* Make sure that this is only adjusted if vehicle really is of type vtol*/
 			if (is_vtol(&status)) {
 				status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
+				status.in_transition_mode = vtol_status.vtol_in_trans_mode;
 			}
 		}
 
@@ -2682,13 +2685,13 @@ set_control_mode()
 			!offboard_control_mode.ignore_velocity ||
 			!offboard_control_mode.ignore_acceleration_force;
 
-		control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity ||
-			!offboard_control_mode.ignore_position;
+		control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity ||
+			!offboard_control_mode.ignore_position) && !vtol_status.vtol_in_trans_mode;
 
 		control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity ||
 			!offboard_control_mode.ignore_position;
 
-		control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position;
+		control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !vtol_status.vtol_in_trans_mode;
 
 		control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity ||
 			!offboard_control_mode.ignore_position;

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -941,7 +941,7 @@ FixedwingAttitudeControl::task_main()
 					att_sp.thrust = throttle_sp;
 
 					/* lazily publish the setpoint only once available */
-					if (!_vehicle_status.is_rotary_wing) {
+					if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
 						if (_attitude_sp_pub != nullptr) {
 							/* publish the attitude setpoint */
 							orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -993,6 +993,11 @@ MulticopterPositionControl::task_main()
 			reset_yaw_sp = true;
 		}
 
+		// XXX Temporary: for vtol use we need to reset the yaw setpoint when we are doing a transition
+		if (_vehicle_status.in_transition_mode) {
+			reset_yaw_sp = true;
+		}
+
 		//Update previous arming state
 		was_armed = _control_mode.flag_armed;
 
@@ -1459,7 +1464,7 @@ MulticopterPositionControl::task_main()
 		if (!(_control_mode.flag_control_offboard_enabled &&
 					!(_control_mode.flag_control_position_enabled ||
 						_control_mode.flag_control_velocity_enabled))) {
-			if (_att_sp_pub != nullptr && _vehicle_status.is_rotary_wing) {
+			if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
 				orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
 			} else if (_att_sp_pub == nullptr && _vehicle_status.is_rotary_wing){
 				_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);

src/modules/vtol_att_control/standard.cpp

@@ -46,12 +46,15 @@ Standard::Standard(VtolAttitudeControl *attc) :
 	VtolType(attc),
 	_flag_enable_mc_motors(true),
 	_pusher_throttle(0.0f),
-	_mc_att_ctl_weight(1.0f),
 	_airspeed_trans_blend_margin(0.0f)
 {
 	_vtol_schedule.flight_mode = MC_MODE;
 	_vtol_schedule.transition_start = 0;
 
+	_mc_roll_weight = 1.0f;
+	_mc_pitch_weight = 1.0f;
+	_mc_yaw_weight = 1.0f;
+
 	_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
 	_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
 	_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
@@ -107,7 +110,9 @@ void Standard::update_vtol_state()
 		if (_vtol_schedule.flight_mode == MC_MODE) {
 			// in mc mode
 			_vtol_schedule.flight_mode = MC_MODE;
-			_mc_att_ctl_weight = 1.0f;
+			_mc_roll_weight = 1.0f;
+			_mc_pitch_weight = 1.0f;
+			_mc_yaw_weight = 1.0f;
 
 		} else if (_vtol_schedule.flight_mode == FW_MODE) {
 			// transition to mc mode
@@ -118,7 +123,9 @@ void Standard::update_vtol_state()
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
 			// failsafe back to mc mode
 			_vtol_schedule.flight_mode = MC_MODE;
-			_mc_att_ctl_weight = 1.0f;
+			_mc_roll_weight = 1.0f;
+			_mc_pitch_weight = 1.0f;
+			_mc_yaw_weight = 1.0f;
 
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
 			// keep transitioning to mc mode
@@ -138,7 +145,9 @@ void Standard::update_vtol_state()
 		} else if (_vtol_schedule.flight_mode == FW_MODE) {
 			// in fw mode
 			_vtol_schedule.flight_mode = FW_MODE;
-			_mc_att_ctl_weight = 0.0f;
+			_mc_roll_weight = 0.0f;
+			_mc_pitch_weight = 0.0f;
+			_mc_yaw_weight = 0.0f;
 
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
@@ -184,18 +193,28 @@ void Standard::update_transition_state()
 
 		// do blending of mc and fw controls if a blending airspeed has been provided
 		if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) {
-			_mc_att_ctl_weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
+			float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
+			_mc_roll_weight = weight;
+			_mc_pitch_weight = weight;
+			_mc_yaw_weight = weight;
 		} else {
 			// at low speeds give full weight to mc
-			_mc_att_ctl_weight = 1.0f;
+			_mc_roll_weight = 1.0f;
+			_mc_pitch_weight = 1.0f;
+			_mc_yaw_weight = 1.0f;
 		}
 
 	} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
 		// continually increase mc attitude control as we transition back to mc mode
 		if (_params_standard.back_trans_dur > 0.0f) {
-			_mc_att_ctl_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
+			float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
+			_mc_roll_weight = weight;
+			_mc_pitch_weight = weight;
+			_mc_yaw_weight = weight;
 		} else {
-			_mc_att_ctl_weight = 1.0f;	
+			_mc_roll_weight = 1.0f;
+			_mc_pitch_weight = 1.0f;
+			_mc_yaw_weight = 1.0f;
 		}
 
 		// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again	
@@ -206,7 +225,9 @@ void Standard::update_transition_state()
 		}
 	}
 
-	_mc_att_ctl_weight = math::constrain(_mc_att_ctl_weight, 0.0f, 1.0f);
+	_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
+	_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
+	_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
 }
 
 void Standard::update_mc_state()
@@ -214,16 +235,6 @@ void Standard::update_mc_state()
 	// do nothing
 }
 
-void Standard::process_mc_data()
-{
-	fill_att_control_output();
-}
-
-void Standard::process_fw_data()
-{
-	fill_att_control_output();
-}
-
  void Standard::update_fw_state()
 {
 	// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again	
@@ -242,27 +253,26 @@ void Standard::update_external_state()
  * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine
  * what proportion of control should be applied to each of the control groups (mc and fw).
  */
-void Standard::fill_att_control_output()
+void Standard::fill_actuator_outputs()
 {
 	/* multirotor controls */
-	_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _mc_att_ctl_weight;	// roll
-	_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _mc_att_ctl_weight;	// pitch
-	_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _mc_att_ctl_weight;	// yaw
-	_actuators_out_0->control[3] = _actuators_mc_in->control[3];	// throttle
+	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;	// roll
+	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;	// pitch
+	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;	// yaw
+	_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];	// throttle
 
 	/* fixed wing controls */
-	const float fw_att_ctl_weight = 1.0f - _mc_att_ctl_weight;
-	_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * fw_att_ctl_weight;	//roll
-	_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim) * fw_att_ctl_weight;	//pitch
-	_actuators_out_1->control[2] = _actuators_fw_in->control[2] * fw_att_ctl_weight;	// yaw
+	_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight);	//roll
+	_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight);	//pitch
+	_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight);	// yaw
 
 	// set the fixed wing throttle control
 	if (_vtol_schedule.flight_mode == FW_MODE) {
 		// take the throttle value commanded by the fw controller
-		_actuators_out_1->control[3] = _actuators_fw_in->control[3];
+		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
 	} else {
 		// otherwise we may be ramping up the throttle during the transition to fw mode 
-		_actuators_out_1->control[3] = _pusher_throttle;
+		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
 	}
 }
 

src/modules/vtol_att_control/standard.h

@@ -57,9 +57,7 @@ public:
 
 	void update_vtol_state();
 	void update_mc_state();
-	void process_mc_data();
 	void update_fw_state();
-	void process_fw_data();
 	void update_transition_state();
 	void update_external_state();
 
@@ -95,10 +93,9 @@ private:
 
 	bool _flag_enable_mc_motors;
 	float _pusher_throttle;
-	float _mc_att_ctl_weight;	// the amount of multicopter attitude control that should be applied in fixed wing mode while transitioning 
 	float _airspeed_trans_blend_margin;
 
-	void fill_att_control_output();
+	void fill_actuator_outputs();
 	void set_max_mc(unsigned pwm_value);
 
 	int parameters_update();

src/modules/vtol_att_control/tailsitter.cpp

@@ -73,13 +73,6 @@ void Tailsitter::update_mc_state()
 	}
 }
 
-void Tailsitter::process_mc_data()
-{
-	// scale pitch control with total airspeed
-	//scale_mc_output();
-	fill_mc_att_control_output();
-}
-
 void Tailsitter::update_fw_state()
 {
 	if (flag_idle_mc) {
@@ -88,11 +81,6 @@ void Tailsitter::update_fw_state()
 	}
 }
 
-void Tailsitter::process_fw_data()
-{
-	fill_fw_att_control_output();
-}
-
 void Tailsitter::update_transition_state()
 {
 
@@ -152,38 +140,41 @@ Tailsitter::scale_mc_output()
 }
 
 /**
-* Prepare message to acutators with data from fw attitude controller.
+* Write data to actuator output topic.
 */
-void Tailsitter::fill_fw_att_control_output()
+void Tailsitter::fill_actuator_outputs()
 {
-	/*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] + _params->fw_pitch_trim;	// 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
-}
+	switch(_vtol_mode) {
+		case ROTARY_WING:
+			_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
+			_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
+			_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
+			_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
 
-/**
-* Prepare message to acutators with data from mc attitude controller.
-*/
-void Tailsitter::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];
+			if (_params->elevons_mc_lock == 1) {
+				_actuators_out_1->control[0] = 0;
+				_actuators_out_1->control[1] = 0;
+			} else {
+				// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
+				_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];	//roll elevon
+				_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];	//pitch elevon
+			}
+			break;
+		case FIXED_WING:
+			// in fixed wing mode we use engines only for providing thrust, no moments are generated
+			_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
+			_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
+			_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
+			_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
 
-	if (_params->elevons_mc_lock == 1) {
-		_actuators_out_1->control[0] = 0;
-		_actuators_out_1->control[1] = 0;
-	} else {
-		_actuators_out_1->control[0] = _actuators_mc_in->control[2];	//roll elevon
-		_actuators_out_1->control[1] = _actuators_mc_in->control[1];	//pitch elevon
+			_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];	// roll elevon
+			_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim;	// pitch elevon
+			_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW];	// yaw
+			_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];	// throttle
+			break;
+		case TRANSITION:
+		case EXTERNAL:
+			// not yet implemented, we are switching brute force at the moment
+			break;
 	}
 }

src/modules/vtol_att_control/tailsitter.h

@@ -53,15 +53,12 @@ public:
 
 	void update_vtol_state();
 	void update_mc_state();
-	void process_mc_data();
 	void update_fw_state();
-	void process_fw_data();
 	void update_transition_state();
 	void update_external_state();
 
 private:
-	void fill_mc_att_control_output();
-	void fill_fw_att_control_output();
+	void fill_actuator_outputs();
 	void calc_tot_airspeed();
 	void scale_mc_output();
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -41,25 +41,34 @@
 #include "tiltrotor.h"
 #include "vtol_att_control_main.h"
 
-#define ARSP_BLEND_START 8.0f	// airspeed at which we start blending mc/fw controls
+#define ARSP_YAW_CTRL_DISABLE 7.0f	// airspeed at which we stop controlling yaw during a front transition
 
 Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
 VtolType(attc),
-flag_max_mc(true),
+_rear_motors(ENABLED),
 _tilt_control(0.0f),
-_roll_weight_mc(1.0f)
+_roll_weight_mc(1.0f),
+_yaw_weight_mc(1.0f),
+_min_front_trans_dur(0.5f)
 {
 	_vtol_schedule.flight_mode = MC_MODE;
 	_vtol_schedule.transition_start = 0;
 
+	_mc_roll_weight = 1.0f;
+	_mc_pitch_weight = 1.0f;
+	_mc_yaw_weight = 1.0f;
+
 	_params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR");
 	_params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR");
 	_params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC");
 	_params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS");
 	_params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW");
 	_params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS");
+	_params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND");
 	_params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
- }
+	_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
+	_params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFF");
+}
 
 Tiltrotor::~Tiltrotor()
 {
@@ -72,6 +81,11 @@ Tiltrotor::parameters_update()
 	float v;
 	int l;
 
+	/* motors that must be turned off when in fixed wing mode */
+	param_get(_params_handles_tiltrotor.fw_motors_off, &l);
+	_params_tiltrotor.fw_motors_off = get_motor_off_channels(l);
+
+
 	/* vtol duration of a front transition */
 	param_get(_params_handles_tiltrotor.front_trans_dur, &v);
 	_params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f);
@@ -96,13 +110,44 @@ Tiltrotor::parameters_update()
 	param_get(_params_handles_tiltrotor.airspeed_trans, &v);
 	_params_tiltrotor.airspeed_trans = v;
 
+	/* vtol airspeed at which we start blending mc/fw controls */
+	param_get(_params_handles_tiltrotor.airspeed_blend_start, &v);
+	_params_tiltrotor.airspeed_blend_start = v;
+
 	/* vtol lock elevons in multicopter */
 	param_get(_params_handles_tiltrotor.elevons_mc_lock, &l);
 	_params_tiltrotor.elevons_mc_lock = l;
 
+	/* vtol front transition phase 2 duration */
+	param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v);
+	_params_tiltrotor.front_trans_dur_p2 = v;
+
+	/* avoid parameters which will lead to zero division in the transition code */
+	_params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur);
+
+	if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) {
+		_params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f;
+	}
+
 	return OK;
 }
 
+int Tiltrotor::get_motor_off_channels(int channels) {
+	int channel_bitmap = 0;
+	
+	int channel;
+	for (int i = 0; i < _params->vtol_motor_count; ++i) {
+		channel = channels % 10;
+		if (channel == 0) {
+			break;
+		}
+		channel_bitmap |= 1 << channel;
+		channels = channels / 10;
+	}
+
+	return channel_bitmap;
+}
+
 void Tiltrotor::update_vtol_state()
 {
  	parameters_update();
@@ -113,53 +158,60 @@ void Tiltrotor::update_vtol_state()
 	 * forward completely. For the backtransition the motors simply rotate back.
  	*/
 
-	if (_manual_control_sp->aux1 < 0.0f && _vtol_schedule.flight_mode == MC_MODE) {
-		// mc mode
-		_vtol_schedule.flight_mode 	= MC_MODE;
-		_tilt_control 			= _params_tiltrotor.tilt_mc;
-		_roll_weight_mc = 1.0f;
-	} else if (_manual_control_sp->aux1 < 0.0f && _vtol_schedule.flight_mode == FW_MODE) {
-		_vtol_schedule.flight_mode 	= TRANSITION_BACK;
-		flag_max_mc = true;
-		_vtol_schedule.transition_start = hrt_absolute_time();
-	} else if (_manual_control_sp->aux1 >= 0.0f && _vtol_schedule.flight_mode == MC_MODE) {
-		// instant of doeing a front-transition
-		_vtol_schedule.flight_mode 	= TRANSITION_FRONT_P1;
-		_vtol_schedule.transition_start = hrt_absolute_time();
-	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 > 0.0f) {
-		// check if we have reached airspeed to switch to fw mode
-		if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
-			_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
-			flag_max_mc = true;
-			_vtol_schedule.transition_start = hrt_absolute_time();
-		}
-	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 > 0.0f) {
-		if (_tilt_control >= _params_tiltrotor.tilt_fw) {
-			_vtol_schedule.flight_mode = FW_MODE;
-			_tilt_control = _params_tiltrotor.tilt_fw;
-		}
-	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 < 0.0f) {
-		// failsave into mc mode
-		_vtol_schedule.flight_mode = MC_MODE;
-		_tilt_control = _params_tiltrotor.tilt_mc;
-	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 < 0.0f) {
-		// failsave into mc mode
-		_vtol_schedule.flight_mode = MC_MODE;
-		_tilt_control = _params_tiltrotor.tilt_mc;
-	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 < 0.0f) {
-		if (_tilt_control <= _params_tiltrotor.tilt_mc) {
-			_vtol_schedule.flight_mode = MC_MODE;
-			_tilt_control = _params_tiltrotor.tilt_mc;
-			flag_max_mc = false;
-		}
-	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 > 0.0f) {
-		// failsave into fw mode
-		_vtol_schedule.flight_mode = FW_MODE;
-		_tilt_control = _params_tiltrotor.tilt_fw;
-	}
+	if (_manual_control_sp->aux1 < 0.0f) {
 
-	// tilt rotors if necessary
-	update_transition_state();
+		// plane is in multicopter mode
+		switch(_vtol_schedule.flight_mode) {
+			case MC_MODE:
+				break;
+			case FW_MODE:
+				_vtol_schedule.flight_mode 	= TRANSITION_BACK;
+				_vtol_schedule.transition_start = hrt_absolute_time();
+				break;
+			case TRANSITION_FRONT_P1:
+				// failsafe into multicopter mode
+				_vtol_schedule.flight_mode = MC_MODE;
+				break;
+			case TRANSITION_FRONT_P2:
+				// failsafe into multicopter mode
+				_vtol_schedule.flight_mode = MC_MODE;
+				break;
+			case TRANSITION_BACK:
+				if (_tilt_control <= _params_tiltrotor.tilt_mc) {
+					_vtol_schedule.flight_mode = MC_MODE;
+				}
+				break;
+		}
+	} else {
+
+		switch(_vtol_schedule.flight_mode) {
+			case MC_MODE:
+				// initialise a front transition
+				_vtol_schedule.flight_mode 	= TRANSITION_FRONT_P1;
+				_vtol_schedule.transition_start = hrt_absolute_time();
+				break;
+			case FW_MODE:
+				break;
+			case TRANSITION_FRONT_P1:
+				// check if we have reached airspeed to switch to fw mode
+				if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
+					_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
+					_vtol_schedule.transition_start = hrt_absolute_time();
+				}
+				break;
+			case TRANSITION_FRONT_P2:
+				// if the rotors have been tilted completely we switch to fw mode
+				if (_tilt_control >= _params_tiltrotor.tilt_fw) {
+					_vtol_schedule.flight_mode = FW_MODE;
+					_tilt_control = _params_tiltrotor.tilt_fw;
+				}
+				break;
+			case TRANSITION_BACK:
+				// failsafe into fixed wing mode
+				_vtol_schedule.flight_mode = FW_MODE;
+				break;
+		}
+	}
 
 	// map tiltrotor specific control phases to simple control modes
 	switch(_vtol_schedule.flight_mode) {
@@ -179,10 +231,12 @@ void Tiltrotor::update_vtol_state()
 
 void Tiltrotor::update_mc_state()
 {
-	// adjust max pwm for rear motors to spin up
-	if (!flag_max_mc) {
-		set_max_mc();
-		flag_max_mc = true;
+	// make sure motors are not tilted
+	_tilt_control = _params_tiltrotor.tilt_mc;
+
+	// enable rear motors
+	if (_rear_motors != ENABLED) {
+		set_rear_motor_state(ENABLED);
 	}
 
 	// set idle speed for rotary wing mode
@@ -190,27 +244,20 @@ void Tiltrotor::update_mc_state()
 		set_idle_mc();
 		flag_idle_mc = true;
 	}
-}
 
-void Tiltrotor::process_mc_data()
-{
-	fill_att_control_output();
+	_mc_roll_weight = 1.0f;
+	_mc_pitch_weight = 1.0f;
+	_mc_yaw_weight = 1.0f;
 }
 
  void Tiltrotor::update_fw_state()
 {
-	/* in fw mode we need the rear motors to stop spinning, in backtransition
-	 * mode we let them spin in idle
-	 */
-	if (flag_max_mc) {
-		if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
-			set_max_fw(1200);
-			set_idle_mc();
-		} else {
-			set_max_fw(950);
-			set_idle_fw();
-		}
-		flag_max_mc = false;
+	// make sure motors are tilted forward
+	_tilt_control = _params_tiltrotor.tilt_fw;
+
+	// disable rear motors
+	if (_rear_motors != DISABLED) {
+		set_rear_motor_state(DISABLED);
 	}
 
 	// adjust idle for fixed wing flight
@@ -218,47 +265,68 @@ void Tiltrotor::process_mc_data()
 		set_idle_fw();
 		flag_idle_mc = false;
 	}
- }
 
-void Tiltrotor::process_fw_data()
-{
-	fill_att_control_output();
+	_mc_roll_weight = 0.0f;
+	_mc_pitch_weight = 0.0f;
+	_mc_yaw_weight = 0.0f;
 }
 
 void Tiltrotor::update_transition_state()
 {
 	if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
+		// for the first part of the transition the rear rotors are enabled
+		if (_rear_motors != ENABLED) {
+			set_rear_motor_state(ENABLED);
+		}
 		// tilt rotors forward up to certain angle
-		if (_params_tiltrotor.front_trans_dur <= 0.0f) {
-			_tilt_control = _params_tiltrotor.tilt_transition;
-		} else if (_tilt_control <= _params_tiltrotor.tilt_transition) {
-			_tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * 
-						(float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f);
+		if (_tilt_control <= _params_tiltrotor.tilt_transition ) {
+			_tilt_control = _params_tiltrotor.tilt_mc +
+				fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f);
 		}
 
 		// do blending of mc and fw controls
-		if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START && _params_tiltrotor.airspeed_trans - ARSP_BLEND_START > 0.0f) {
-			_roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START);
+		if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) {
+			_mc_roll_weight = 0.0f;
 		} else {
 			// at low speeds give full weight to mc
-			_roll_weight_mc = 1.0f;
+			_mc_roll_weight = 1.0f;
 		}
 
-		_roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f);
+		// disable mc yaw control once the plane has picked up speed
+		_mc_yaw_weight = 1.0f;
+		if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
+			_mc_yaw_weight = 0.0f;
+		}
 
 	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
-		_tilt_control = _params_tiltrotor.tilt_transition +  fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) *
-					(float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (0.5f * 1000000.0f);
-		_roll_weight_mc = 0.0f;
+		// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
+		_tilt_control = _params_tiltrotor.tilt_transition +
+			fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
+		_mc_roll_weight = 0.0f;
 	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
-		// tilt rotors forward up to certain angle
-		float progress = (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f);
-		if (_tilt_control > _params_tiltrotor.tilt_mc) {
-			_tilt_control = _params_tiltrotor.tilt_fw -  fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * progress;
+		if (_rear_motors != IDLE) {
+			set_rear_motor_state(IDLE);
 		}
 
-		_roll_weight_mc = progress;
+		if (!flag_idle_mc) {
+			set_idle_mc();
+			flag_idle_mc = true;
+		}
+		// tilt rotors back
+		if (_tilt_control > _params_tiltrotor.tilt_mc) {
+			_tilt_control = _params_tiltrotor.tilt_fw -
+				fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f);
+		}
+
+		// set zero throttle for backtransition otherwise unwanted moments will be created
+		_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
+
+		_mc_roll_weight = 0.0f;
+
 	}
+
+	_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
+	_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
 }
 
 void Tiltrotor::update_external_state()
@@ -266,35 +334,47 @@ void Tiltrotor::update_external_state()
 
 }
 
- /**
-* Prepare message to acutators with data from the attitude controllers.
+/**
+* Write data to actuator output topic.
 */
-void Tiltrotor::fill_att_control_output()
+void Tiltrotor::fill_actuator_outputs()
 {
-	_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc;	// roll
-	_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc;	// pitch
-	_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc;	// yaw
+	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
+	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
+	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
+	_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
 
-	if (_vtol_schedule.flight_mode == FW_MODE) {
-		_actuators_out_1->control[3] = _actuators_fw_in->control[3];	// fw throttle
-	} else {
-		_actuators_out_0->control[3] = _actuators_mc_in->control[3];	// mc throttle
-	}
+	_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight);
 
-	_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc);	//roll elevon
-	_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc);	//pitch elevon	
-	_actuators_out_1->control[4] = _tilt_control;	// for tilt-rotor control
-
-	// unused now but still logged
-	_actuators_out_1->control[2] = _actuators_fw_in->control[2];	// fw yaw
+	_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);
+	_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight);	// yaw
+	_actuators_out_1->control[4] = _tilt_control;
 }
 
+
 /**
-* Kill rear motors for the FireFLY6 when in fw mode.
+* Set state of rear motors.
 */
-void
-Tiltrotor::set_max_fw(unsigned pwm_value)
-{
+
+void Tiltrotor::set_rear_motor_state(rear_motor_state state) {
+	int pwm_value = PWM_DEFAULT_MAX;
+
+	// map desired rear rotor state to max allowed pwm signal
+	switch (state) {
+		case ENABLED:
+			pwm_value = PWM_DEFAULT_MAX;
+			_rear_motors = ENABLED;
+			break;
+		case DISABLED:
+			pwm_value = PWM_LOWEST_MAX;
+			_rear_motors = DISABLED;
+			break;
+		case IDLE:
+			pwm_value = _params->idle_pwm_mc;
+			_rear_motors = IDLE;
+			break;
+	}
+
 	int ret;
 	unsigned servo_count;
 	char *dev = PWM_OUTPUT0_DEVICE_PATH;
@@ -307,10 +387,10 @@ Tiltrotor::set_max_fw(unsigned pwm_value)
 	memset(&pwm_values, 0, sizeof(pwm_values));
 
 	for (int i = 0; i < _params->vtol_motor_count; i++) {
-		if (i == 2 || i == 3) {
+		if (is_motor_off_channel(i)) {
 			pwm_values.values[i] = pwm_value;
 		} else {
-			pwm_values.values[i] = 2000;
+			pwm_values.values[i] = PWM_DEFAULT_MAX;
 		}
 		pwm_values.channel_count = _params->vtol_motor_count;
 	}
@@ -322,28 +402,6 @@ Tiltrotor::set_max_fw(unsigned pwm_value)
 	close(fd);
 }
 
-void
-Tiltrotor::set_max_mc()
-{
-	int ret;
-	unsigned servo_count;
-	char *dev = PWM_OUTPUT0_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);
-	struct pwm_output_values pwm_values;
-	memset(&pwm_values, 0, sizeof(pwm_values));
-
-	for (int i = 0; i < _params->vtol_motor_count; i++) {
-		pwm_values.values[i] = 2000;
-		pwm_values.channel_count = _params->vtol_motor_count;
-	}
-
-	ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
-
-	if (ret != OK) {errx(ret, "failed setting max values");}
-
-	close(fd);
+bool Tiltrotor::is_motor_off_channel(const int channel) {
+	return (_params_tiltrotor.fw_motors_off >> channel) & 1;
 }

src/modules/vtol_att_control/tiltrotor.h

@@ -52,24 +52,44 @@ public:
 	Tiltrotor(VtolAttitudeControl * _att_controller);
 	~Tiltrotor();
 
+	/**
+	 * Update vtol state.
+	 */
 	void update_vtol_state();
+
+	/**
+	 * Update multicopter state.
+	 */
 	void update_mc_state();
-	void process_mc_data();
+
+	/**
+	 * Update fixed wing state.
+	 */
 	void update_fw_state();
-	void process_fw_data();
+
+	/**
+	 * Update transition state.
+	 */
 	void update_transition_state();
+
+	/**
+	 * Update external state.
+	 */
 	void update_external_state();
 
 private:
 
 	struct {
-		float front_trans_dur;
-		float back_trans_dur;
-		float tilt_mc;
-		float tilt_transition;
-		float tilt_fw;
-		float airspeed_trans;
-		int elevons_mc_lock;			// lock elevons in multicopter mode
+		float front_trans_dur;			/**< duration of first part of front transition */
+		float back_trans_dur;			/**< duration of back transition */
+		float tilt_mc;					/**< actuator value corresponding to mc tilt */
+		float tilt_transition;			/**< actuator value corresponding to transition tilt (e.g 45 degrees) */
+		float tilt_fw;					/**< actuator value corresponding to fw tilt */
+		float airspeed_trans;			/**< airspeed at which we switch to fw mode after transition */
+		float airspeed_blend_start;		/**< airspeed at which we start blending mc/fw controls */
+		int elevons_mc_lock;			/**< lock elevons in multicopter mode */
+		float front_trans_dur_p2;
+		int fw_motors_off;			/**< bitmask of all motors that should be off in fixed wing mode */
 	} _params_tiltrotor;
 
 	struct {
@@ -79,30 +99,65 @@ private:
 		param_t tilt_transition;
 		param_t tilt_fw;
 		param_t airspeed_trans;
+		param_t airspeed_blend_start;
 		param_t elevons_mc_lock;
+		param_t front_trans_dur_p2;
+		param_t fw_motors_off;
 	} _params_handles_tiltrotor;
 
 	enum vtol_mode {
-		MC_MODE = 0,
-		TRANSITION_FRONT_P1,
-		TRANSITION_FRONT_P2,
-		TRANSITION_BACK,
-		FW_MODE
+		MC_MODE = 0,			/**< vtol is in multicopter mode */
+		TRANSITION_FRONT_P1,	/**< vtol is in front transition part 1 mode */
+		TRANSITION_FRONT_P2,	/**< vtol is in front transition part 2 mode */
+		TRANSITION_BACK,		/**< vtol is in back transition mode */
+		FW_MODE					/**< vtol is in fixed wing mode */
 	};
 
+	/**
+	 * Specific to tiltrotor with vertical aligned rear engine/s.
+	 * These engines need to be shut down in fw mode. During the back-transition
+	 * they need to idle otherwise they need too much time to spin up for mc mode.
+	 */
+	enum rear_motor_state {
+		ENABLED = 0,
+		DISABLED,
+		IDLE
+	} _rear_motors;
+
 	struct {
-		vtol_mode flight_mode;			// indicates in which mode the vehicle is in
-		hrt_abstime transition_start;	// at what time did we start a transition (front- or backtransition)
+		vtol_mode flight_mode;			/**< vtol flight mode, defined by enum vtol_mode */
+		hrt_abstime transition_start;	/**< absoulte time at which front transition started */
 	}_vtol_schedule;
 
-	bool flag_max_mc;
-	float _tilt_control;
-	float _roll_weight_mc;
+	float _tilt_control;		/**< actuator value for the tilt servo */
+	float _roll_weight_mc;		/**< multicopter desired roll moment weight */
+	float _yaw_weight_mc;		/**< multicopter desired yaw moment weight */
 
-	void fill_att_control_output();
-	void set_max_mc();
-	void set_max_fw(unsigned pwm_value);
+	const float _min_front_trans_dur;	/**< min possible time in which rotors are rotated into the first position */
 
+	/**
+	 * Return a bitmap of channels that should be turned off in fixed wing mode.
+	 */
+	int get_motor_off_channels(const int channels);
+
+	/**
+	 * Return true if the motor channel is off in fixed wing mode.
+	 */
+	bool is_motor_off_channel(const int channel);
+
+	/**
+	 * Write control values to actuator output topics.
+	 */
+	void fill_actuator_outputs();
+
+	/**
+	 * Adjust the state of the rear motors. In fw mode they shouldn't spin.
+	 */
+	void set_rear_motor_state(rear_motor_state state);
+
+	/**
+	 * Update parameters.
+	 */
 	int parameters_update();
 
 };

src/modules/vtol_att_control/tiltrotor_params.c

@@ -72,3 +72,25 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f);
+
+/**
+ * Duration of front transition phase 2
+ *
+ * Time in seconds it should take for the rotors to rotate forward completely from the point
+ * when the plane has picked up enough airspeed and is ready to go into fixed wind mode.
+ *
+ * @min 0.1
+ * @max 2
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);
+
+/**
+ * The channel number of motors that must be turned off in fixed wing mode.
+ *
+ *
+ * @min 0
+ * @max 123456
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_FW_MOT_OFF, 0);

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -43,7 +43,6 @@
  * @author Thomas Gubler	<thomasgubler@gmail.com>
  *
  */
-
 #include "vtol_att_control_main.h"
 
 namespace VTOL_att_control
@@ -492,34 +491,34 @@ void VtolAttitudeControl::task_main()
 		if (_vtol_type->get_mode() == ROTARY_WING) {
 			// vehicle is in rotary wing mode
 			_vtol_vehicle_status.vtol_in_rw_mode = true;
-
-			_vtol_type->update_mc_state();
+			_vtol_vehicle_status.vtol_in_trans_mode = false;
 
 			// got data from mc attitude controller
 			if (fds[0].revents & POLLIN) {
 				orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
 
-				_vtol_type->process_mc_data();
+				_vtol_type->update_mc_state();
 
 				fill_mc_att_rates_sp();
 			}
 		} else if (_vtol_type->get_mode() == FIXED_WING) {
 			// vehicle is in fw mode
 			_vtol_vehicle_status.vtol_in_rw_mode = false;
-
-			_vtol_type->update_fw_state();
+			_vtol_vehicle_status.vtol_in_trans_mode = false;
 
 			// got data from fw attitude controller
 			if (fds[1].revents & POLLIN) {
 				orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
 				vehicle_manual_poll();
 
-				_vtol_type->process_fw_data();
+				_vtol_type->update_fw_state();
 
 				fill_fw_att_rates_sp();
 			}
 		} else if (_vtol_type->get_mode() == TRANSITION) {
 			// vehicle is doing a transition
+			_vtol_vehicle_status.vtol_in_trans_mode = true;
+
 			bool got_new_data = false;
 			if (fds[0].revents & POLLIN) {
 				orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
@@ -534,8 +533,6 @@ void VtolAttitudeControl::task_main()
 			// update transition state if got any new data
 			if (got_new_data) {
 				_vtol_type->update_transition_state();
-
-				_vtol_type->process_mc_data();
 				fill_mc_att_rates_sp();
 			}
 
@@ -544,6 +541,7 @@ void VtolAttitudeControl::task_main()
 			_vtol_type->update_external_state();
 		}
 
+		_vtol_type->fill_actuator_outputs();
 
 		/* Only publish if the proper mode(s) are enabled */
 		if(_v_control_mode.flag_control_attitude_enabled ||

src/modules/vtol_att_control/vtol_type.h

@@ -41,6 +41,8 @@
 #ifndef VTOL_TYPE_H
 #define VTOL_TYPE_H
 
+#include <lib/mathlib/mathlib.h>
+
 struct Params {
 	int idle_pwm_mc;			// pwm value for idle in mc mode
 	int vtol_motor_count;		// number of motors
@@ -75,11 +77,10 @@ public:
 
 	virtual void update_vtol_state() = 0;
 	virtual void update_mc_state() = 0;
-	virtual void process_mc_data() = 0;
 	virtual void update_fw_state() = 0;
-	virtual void process_fw_data() = 0;
 	virtual void update_transition_state() = 0;
 	virtual void update_external_state() = 0;
+	virtual void fill_actuator_outputs() = 0;
 
 	void set_idle_mc();
 	void set_idle_fw();
@@ -111,6 +112,10 @@ protected:
 
 	bool flag_idle_mc;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
 
+	float _mc_roll_weight;	// weight for multicopter attitude controller roll output
+	float _mc_pitch_weight;	// weight for multicopter attitude controller pitch output
+	float _mc_yaw_weight;	// weight for multicopter attitude controller yaw output
+
 };
 
 #endif