VTOL rate control architecture improvements (#10819)

* attitude and rate setpoint message: use 3D array for thrust demand
* FixedWingAttitudeControl: rework airspeed scaling
  * move airspeed and scaling calculation into separate method
  * if vtol in hover and airspeed disabled use minimum airspeed instead of trim airspeed

src/modules/vtol_att_control/standard.cpp

@@ -46,13 +46,10 @@
 
 #include <float.h>
 
-using matrix::wrap_pi;
+using namespace matrix;
 
 Standard::Standard(VtolAttitudeControl *attc) :
-	VtolType(attc),
-	_pusher_throttle(0.0f),
-	_reverse_output(0.0f),
-	_airspeed_trans_blend_margin(0.0f)
+	VtolType(attc)
 {
 	_vtol_schedule.flight_mode = MC_MODE;
 	_vtol_schedule.transition_start = 0;
@@ -156,8 +153,8 @@ void Standard::update_vtol_state()
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
 			// transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed
 
-			const matrix::Dcmf R_to_body(matrix::Quatf(_v_att->q).inversed());
-			const matrix::Vector3f vel = R_to_body * matrix::Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
+			const Dcmf R_to_body(Quatf(_v_att->q).inversed());
+			const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
 
 			float x_vel = vel(0);
 
@@ -261,7 +258,7 @@ void Standard::update_transition_state()
 
 		// ramp up FW_PSP_OFF
 		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);
-		matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
 		q_sp.copyTo(_v_att_sp->q_d);
 		_v_att_sp->q_d_valid = true;
 
@@ -277,7 +274,7 @@ void Standard::update_transition_state()
 
 		// maintain FW_PSP_OFF
 		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset;
-		matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
 		q_sp.copyTo(_v_att_sp->q_d);
 		_v_att_sp->q_d_valid = true;
 
@@ -333,18 +330,18 @@ void Standard::update_mc_state()
 		return;
 	}
 
-	matrix::Dcmf R(matrix::Quatf(_v_att->q));
-	matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d));
-	matrix::Eulerf euler(R);
-	matrix::Eulerf euler_sp(R_sp);
+	const Dcmf R(Quatf(_v_att->q));
+	const Dcmf R_sp(Quatf(_v_att_sp->q_d));
+	const Eulerf euler(R);
+	const Eulerf euler_sp(R_sp);
 	_pusher_throttle = 0.0f;
 
 	// direction of desired body z axis represented in earth frame
-	matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+	Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
 
 	// rotate desired body z axis into new frame which is rotated in z by the current
 	// heading of the vehicle. we refer to this as the heading frame.
-	matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2));
+	Dcmf R_yaw = Eulerf(0.0f, 0.0f, -euler(2));
 	body_z_sp = R_yaw * body_z_sp;
 	body_z_sp.normalize();
 
@@ -365,25 +362,25 @@ void Standard::update_mc_state()
 		float pitch_new = 0.0f;
 
 		// create corrected desired body z axis in heading frame
-		matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f);
-		matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
+		const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f);
+		Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
 
 		// rotate the vector into a new frame which is rotated in z by the desired heading
 		// with respect to the earh frame.
 		const float yaw_error = wrap_pi(euler_sp(2) - euler(2));
-		matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error);
+		const Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error);
 		tilt_new = R_yaw_correction * tilt_new;
 
 		// now extract roll and pitch setpoints
 		_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
 		_v_att_sp->roll_body = -asinf(tilt_new(1));
-		R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2));
-		matrix::Quatf q_sp(R_sp);
+
+		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)));
 		q_sp.copyTo(_v_att_sp->q_d);
+		_v_att_sp->q_d_valid = true;
 	}
 
 	_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
-
 }
 
 void Standard::update_fw_state()
@@ -475,5 +472,5 @@ void
 Standard::waiting_on_tecs()
 {
 	// keep thrust from transition
-	_v_att_sp->thrust = _pusher_throttle;
+	_v_att_sp->thrust_body[0] = _pusher_throttle;
 };

src/modules/vtol_att_control/standard.h

@@ -55,14 +55,14 @@ class Standard : public VtolType
 public:
 
 	Standard(VtolAttitudeControl *_att_controller);
-	~Standard() = default;
+	~Standard() override = default;
 
-	virtual void update_vtol_state();
-	virtual void update_transition_state();
-	virtual void update_fw_state();
-	virtual void update_mc_state();
-	virtual void fill_actuator_outputs();
-	virtual void waiting_on_tecs();
+	void update_vtol_state() override;
+	void update_transition_state() override;
+	void update_fw_state() override;
+	void update_mc_state() override;
+	void fill_actuator_outputs() override;
+	void waiting_on_tecs() override;
 
 private:
 
@@ -98,10 +98,10 @@ private:
 		hrt_abstime transition_start;	// at what time did we start a transition (front- or backtransition)
 	} _vtol_schedule;
 
-	float _pusher_throttle;
-	float _reverse_output;
-	float _airspeed_trans_blend_margin;
+	float _pusher_throttle{0.0f};
+	float _reverse_output{0.0f};
+	float _airspeed_trans_blend_margin{0.0f};
 
-	virtual void parameters_update();
+	void parameters_update() override;
 };
 #endif

src/modules/vtol_att_control/tailsitter.cpp

@@ -47,11 +47,10 @@
 #define PITCH_TRANSITION_FRONT_P1 -1.1f	// pitch angle to switch to TRANSITION_P2
 #define PITCH_TRANSITION_BACK -0.25f	// pitch angle to switch to MC
 
+using namespace matrix;
+
 Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
-	VtolType(attc),
-	_thrust_transition_start(0.0f),
-	_yaw_transition(0.0f),
-	_pitch_transition_start(0.0f)
+	VtolType(attc)
 {
 	_vtol_schedule.flight_mode = MC_MODE;
 	_vtol_schedule.transition_start = 0;
@@ -63,6 +62,7 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
 	_flag_was_in_trans_mode = false;
 
 	_params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
+	_params_handles_tailsitter.fw_pitch_sp_offset = param_find("FW_PSP_OFF");
 }
 
 void
@@ -73,18 +73,20 @@ Tailsitter::parameters_update()
 	/* vtol front transition phase 2 duration */
 	param_get(_params_handles_tailsitter.front_trans_dur_p2, &v);
 	_params_tailsitter.front_trans_dur_p2 = v;
+
+	param_get(_params_handles_tailsitter.fw_pitch_sp_offset, &v);
+	_params_tailsitter.fw_pitch_sp_offset = math::radians(v);
 }
 
 void Tailsitter::update_vtol_state()
 {
-
 	/* simple logic using a two way switch to perform transitions.
 	 * after flipping the switch the vehicle will start tilting in MC control mode, picking up
 	 * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode.
 	 * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle.
 	*/
 
-	matrix::Eulerf euler = matrix::Quatf(_v_att->q);
+	Eulerf euler = Quatf(_v_att->q);
 	float pitch = euler.theta();
 
 	if (!_attc->is_fixed_wing_requested()) {
@@ -104,9 +106,10 @@ void Tailsitter::update_vtol_state()
 			break;
 
 		case TRANSITION_BACK:
+			float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 			// check if we have reached pitch angle to switch to MC mode
-			if (pitch >= PITCH_TRANSITION_BACK) {
+			if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _params->back_trans_duration) {
 				_vtol_schedule.flight_mode = MC_MODE;
 			}
 
@@ -176,89 +179,94 @@ void Tailsitter::update_transition_state()
 	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 	if (!_flag_was_in_trans_mode) {
-		// save desired heading for transition and last thrust value
-		_yaw_transition = _v_att_sp->yaw_body;
-		//transition should start from current attitude instead of current setpoint
-		matrix::Eulerf euler = matrix::Quatf(_v_att->q);
-		_pitch_transition_start = euler.theta();
-		_thrust_transition_start = _v_att_sp->thrust;
 		_flag_was_in_trans_mode = true;
+
+		if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
+			// calculate rotation axis for transition.
+			_q_trans_start = Quatf(_v_att->q);
+			Vector3f z = -_q_trans_start.dcm_z();
+			_trans_rot_axis = z.cross(Vector3f(0, 0, -1));
+
+			// as heading setpoint we choose the heading given by the direction the vehicle points
+			float yaw_sp = atan2f(z(1), z(0));
+
+			// the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint,
+			// the yaw setpoint and zero roll since we want wings level transition
+			_q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
+
+			// attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the
+			// multirotor frame
+			_q_trans_start = _q_trans_start * Quatf(Eulerf(0, -M_PI_2_F, 0));
+
+		} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
+			// initial attitude setpoint for the transition should be with wings level
+			_q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body);
+			Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1, 0, 0);
+			_trans_rot_axis = -x.cross(Vector3f(0, 0, -1));
+		}
+
+		_q_trans_sp = _q_trans_start;
 	}
 
+	// tilt angle (zero if vehicle nose points up (hover))
+	float tilt = acosf(_q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - _q_trans_sp(2) * _q_trans_sp(
+				   2) + _q_trans_sp(3) * _q_trans_sp(3));
+
 	if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
 
-		// create time dependant pitch angle set point + 0.2 rad overlap over the switch value
-		_v_att_sp->pitch_body = _pitch_transition_start	- fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) *
-					time_since_trans_start / _params->front_trans_duration;
-		_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, PITCH_TRANSITION_FRONT_P1 - 0.2f,
-							_pitch_transition_start);
+		const float trans_pitch_rate = M_PI_2_F / _params->front_trans_duration;
 
-		// disable mc yaw control once the plane has picked up speed
-		if (_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
-			_mc_yaw_weight = 0.0f;
-
-		} else {
-			_mc_yaw_weight = 1.0f;
+		if (tilt < M_PI_2_F - _params_tailsitter.fw_pitch_sp_offset) {
+			_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
+						       time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
 		}
 
-		_mc_roll_weight = 1.0f;
-		_mc_pitch_weight = 1.0f;
-
 	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
 
+		const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration;
+
 		if (!flag_idle_mc) {
 			flag_idle_mc = set_idle_mc();
 		}
 
-		// create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value
-		_v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) *
-					time_since_trans_start / _params->back_trans_duration;
-		_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, -2.0f, PITCH_TRANSITION_BACK + 0.2f);
-
-		// keep yaw disabled
-		_mc_yaw_weight = 0.0f;
-
-		// smoothly move control weight to MC
-		_mc_roll_weight = _mc_pitch_weight = time_since_trans_start / _params->back_trans_duration;
-
+		if (tilt > 0.01f) {
+			_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
+						       time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
+		}
 	}
 
-	if (_v_control_mode->flag_control_climb_rate_enabled) {
-		_v_att_sp->thrust = _params->front_trans_throttle;
+	_v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2];
 
-	} else {
-		_v_att_sp->thrust = _mc_virtual_att_sp->thrust;
-	}
-
-	_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
-	_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
-	_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
-
-	// compute desired attitude and thrust setpoint for the transition
+	_mc_roll_weight = 1.0f;
+	_mc_pitch_weight = 1.0f;
+	_mc_yaw_weight = 1.0f;
 
 	_v_att_sp->timestamp = hrt_absolute_time();
-	_v_att_sp->roll_body = 0.0f;
-	_v_att_sp->yaw_body = _yaw_transition;
 
-	matrix::Quatf q_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
-	q_sp.copyTo(_v_att_sp->q_d);
+	const Eulerf euler_sp(_q_trans_sp);
+	_v_att_sp->roll_body = euler_sp.phi();
+	_v_att_sp->pitch_body = euler_sp.theta();
+	_v_att_sp->yaw_body = euler_sp.psi();
+
+	_q_trans_sp.copyTo(_v_att_sp->q_d);
 	_v_att_sp->q_d_valid = true;
 }
 
 void Tailsitter::waiting_on_tecs()
 {
 	// copy the last trust value from the front transition
-	_v_att_sp->thrust = _thrust_transition;
-}
-
-void Tailsitter::update_mc_state()
-{
-	VtolType::update_mc_state();
+	_v_att_sp->thrust_body[0] = _thrust_transition;
 }
 
 void Tailsitter::update_fw_state()
 {
 	VtolType::update_fw_state();
+
+	// allow fw yawrate control via multirotor roll actuation. this is useful for vehicles
+	// which don't have a rudder to coordinate turns
+	if (_params->diff_thrust == 1) {
+		_mc_roll_weight = 1.0f;
+	}
 }
 
 /**
@@ -272,67 +280,30 @@ void Tailsitter::fill_actuator_outputs()
 	_actuators_out_1->timestamp = hrt_absolute_time();
 	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
 
-	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];
+	_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;
 
-		if (_params->elevons_mc_lock) {
-			_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;
+	if (_vtol_schedule.flight_mode == FW_MODE) {
 		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
 			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
 
+	} else {
+		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
+	}
+
+	if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == MC_MODE) {
+		_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0;
+		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0;
+
+	} else {
 		_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
-			-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];	// roll elevon
+			-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
 		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
-			_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH];	// 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_TO_FW:
-	case TRANSITION_TO_MC:
-		// in transition engines are mixed by weight (BACK TRANSITION ONLY)
-		_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];
-
-		// 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_fw_in->control[actuator_controls_s::INDEX_ROLL]
-				* (1 - _mc_yaw_weight);
-		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
-			_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
-		// **LATER** + (_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_THROTTLE] =
-			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
-		break;
+			_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH];
 	}
 }

src/modules/vtol_att_control/tailsitter.h

@@ -46,30 +46,32 @@
 #include <perf/perf_counter.h>  /** is it necsacery? **/
 #include <parameters/param.h>
 #include <drivers/drv_hrt.h>
+#include <matrix/matrix/math.hpp>
 
 class Tailsitter : public VtolType
 {
 
 public:
 	Tailsitter(VtolAttitudeControl *_att_controller);
-	~Tailsitter() = default;
+	~Tailsitter() override = default;
 
-	virtual void update_vtol_state();
-	virtual void update_transition_state();
-	virtual void update_mc_state();
-	virtual void update_fw_state();
-	virtual void fill_actuator_outputs();
-	virtual void waiting_on_tecs();
+	void update_vtol_state() override;
+	void update_transition_state() override;
+	void update_fw_state() override;
+	void fill_actuator_outputs() override;
+	void waiting_on_tecs() override;
 
 private:
 
 	struct {
 		float front_trans_dur_p2;
-	} _params_tailsitter;
+		float fw_pitch_sp_offset;
+	} _params_tailsitter{};
 
 	struct {
 		param_t front_trans_dur_p2;
-	} _params_handles_tailsitter;
+		param_t fw_pitch_sp_offset;
+	} _params_handles_tailsitter{};
 
 	enum vtol_mode {
 		MC_MODE = 0,			/**< vtol is in multicopter mode */
@@ -83,14 +85,11 @@ private:
 		hrt_abstime transition_start;	/**< absoulte time at which front transition started */
 	} _vtol_schedule;
 
-	float _thrust_transition_start; // throttle value when we start the front transition
-	float _yaw_transition;	// yaw angle in which transition will take place
-	float _pitch_transition_start;  // pitch angle at the start of transition (tailsitter)
+	matrix::Quatf _q_trans_start;
+	matrix::Quatf _q_trans_sp;
+	matrix::Vector3f _trans_rot_axis;
 
-	/**
-	 * Update parameters.
-	 */
-	virtual void parameters_update();
+	void parameters_update() override;
 
 };
 #endif

src/modules/vtol_att_control/tiltrotor.cpp

@@ -45,8 +45,7 @@
 #define ARSP_YAW_CTRL_DISABLE 7.0f	// airspeed at which we stop controlling yaw during a front transition
 
 Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
-	VtolType(attc),
-	_tilt_control(0.0f)
+	VtolType(attc)
 {
 	_vtol_schedule.flight_mode = MC_MODE;
 	_vtol_schedule.transition_start = 0;
@@ -61,8 +60,6 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
 	_params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS");
 	_params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW");
 	_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
-	_params_handles_tiltrotor.diff_thrust = param_find("VT_FW_DIFTHR_EN");
-	_params_handles_tiltrotor.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
 }
 
 void
@@ -85,16 +82,10 @@ Tiltrotor::parameters_update()
 	/* vtol front transition phase 2 duration */
 	param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v);
 	_params_tiltrotor.front_trans_dur_p2 = v;
-
-	param_get(_params_handles_tiltrotor.diff_thrust, &_params_tiltrotor.diff_thrust);
-
-	param_get(_params_handles_tiltrotor.diff_thrust_scale, &v);
-	_params_tiltrotor.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
 }
 
 void Tiltrotor::update_vtol_state()
 {
-
 	/* simple logic using a two way switch to perform transitions.
 	 * after flipping the switch the vehicle will start tilting rotors, picking up
 	 * forward speed. After the vehicle has picked up enough speed the rotors are tilted
@@ -270,7 +261,7 @@ void Tiltrotor::update_transition_state()
 			_mc_yaw_weight = _mc_roll_weight;
 		}
 
-		_thrust_transition = _mc_virtual_att_sp->thrust;
+		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
 
 	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
 		// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
@@ -289,7 +280,7 @@ void Tiltrotor::update_transition_state()
 		_motor_state = set_motor_state(_motor_state, VALUE, rear_value);
 
 
-		_thrust_transition = _mc_virtual_att_sp->thrust;
+		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
 
 	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
 		if (_motor_state != ENABLED) {
@@ -335,7 +326,7 @@ void Tiltrotor::update_transition_state()
 void Tiltrotor::waiting_on_tecs()
 {
 	// keep multicopter thrust until we get data from TECS
-	_v_att_sp->thrust = _thrust_transition;
+	_v_att_sp->thrust_body[0] = _thrust_transition;
 }
 
 /**
@@ -358,9 +349,9 @@ void Tiltrotor::fill_actuator_outputs()
 			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
 
 		/* allow differential thrust if enabled */
-		if (_params_tiltrotor.diff_thrust == 1) {
+		if (_params->diff_thrust == 1) {
 			_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] =
-				_actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * _params_tiltrotor.diff_thrust_scale;
+				_actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
 		}
 
 	} else {

src/modules/vtol_att_control/tiltrotor.h

@@ -50,14 +50,14 @@ class Tiltrotor : public VtolType
 public:
 
 	Tiltrotor(VtolAttitudeControl *_att_controller);
-	~Tiltrotor() = default;
+	~Tiltrotor() override = default;
 
-	virtual void update_vtol_state();
-	virtual void update_transition_state();
-	virtual void fill_actuator_outputs();
-	virtual void update_mc_state();
-	virtual void update_fw_state();
-	virtual void waiting_on_tecs();
+	void update_vtol_state() override;
+	void update_transition_state() override;
+	void fill_actuator_outputs() override;
+	void update_mc_state() override;
+	void update_fw_state() override;
+	void waiting_on_tecs() override;
 
 private:
 
@@ -66,8 +66,6 @@ private:
 		float tilt_transition;			/**< actuator value corresponding to transition tilt (e.g 45 degrees) */
 		float tilt_fw;					/**< actuator value corresponding to fw tilt */
 		float front_trans_dur_p2;
-		int32_t diff_thrust;
-		float diff_thrust_scale;
 	} _params_tiltrotor;
 
 	struct {
@@ -75,8 +73,6 @@ private:
 		param_t tilt_transition;
 		param_t tilt_fw;
 		param_t front_trans_dur_p2;
-		param_t diff_thrust;
-		param_t diff_thrust_scale;
 	} _params_handles_tiltrotor;
 
 	enum vtol_mode {
@@ -99,12 +95,9 @@ private:
 		hrt_abstime transition_start;	/**< absoulte time at which front transition started */
 	} _vtol_schedule;
 
-	float _tilt_control;		/**< actuator value for the tilt servo */
+	float _tilt_control{0.0f};		/**< actuator value for the tilt servo */
 
-	/**
-	 * Update parameters.
-	 */
-	virtual void parameters_update();
+	void parameters_update() override;
 
 };
 #endif

src/modules/vtol_att_control/tiltrotor_params.c

@@ -85,28 +85,3 @@ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);
-
-/**
- * Differential thrust in forwards flight.
- *
- * Set to 1 to enable differential thrust in fixed-wing flight.
- *
- * @min 0
- * @max 1
- * @decimal 0
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0);
-
-/**
- * Differential thrust scaling factor
- *
- * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight.
- *
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @increment 0.1
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f);

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -86,6 +86,8 @@ VtolAttitudeControl::VtolAttitudeControl()
 	_params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
 	_params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
 	_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
+	_params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN");
+	_params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -486,6 +488,10 @@ VtolAttitudeControl::parameters_update()
 	param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout);
 	param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise);
 	param_get(_params_handles.fw_motors_off, &_params.fw_motors_off);
+	param_get(_params_handles.diff_thrust, &_params.diff_thrust);
+
+	param_get(_params_handles.diff_thrust_scale, &v);
+	_params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
 
 	// standard vtol always needs to turn all mc motors off when going into fixed wing mode
 	// normally the parameter fw_motors_off can be used to specify this, however, since historically standard vtol code
@@ -506,52 +512,6 @@ VtolAttitudeControl::parameters_update()
 	return OK;
 }
 
-/**
-* Prepare message for mc attitude rates setpoint topic
-*/
-void VtolAttitudeControl::fill_mc_att_rates_sp()
-{
-	bool updated;
-	orb_check(_mc_virtual_v_rates_sp_sub, &updated);
-
-	if (updated) {
-		vehicle_rates_setpoint_s v_rates_sp;
-
-		if (orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) {
-			// publish the attitude rates setpoint
-			if (_v_rates_sp_pub != nullptr) {
-				orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp);
-
-			} else {
-				_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp);
-			}
-		}
-	}
-}
-
-/**
-* Prepare message for fw attitude rates setpoint topic
-*/
-void VtolAttitudeControl::fill_fw_att_rates_sp()
-{
-	bool updated;
-	orb_check(_fw_virtual_v_rates_sp_sub, &updated);
-
-	if (updated) {
-		vehicle_rates_setpoint_s v_rates_sp;
-
-		if (orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) {
-			// publish the attitude rates setpoint
-			if (_v_rates_sp_pub != nullptr) {
-				orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp);
-
-			} else {
-				_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp);
-			}
-		}
-	}
-}
-
 int
 VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
@@ -567,8 +527,6 @@ void VtolAttitudeControl::task_main()
 	_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
 	_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint));
-	_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
-	_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
 	_v_att_sub             = orb_subscribe(ORB_ID(vehicle_attitude));
 	_v_control_mode_sub    = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub            = orb_subscribe(ORB_ID(parameter_update));
@@ -680,7 +638,6 @@ void VtolAttitudeControl::task_main()
 
 			// got data from mc attitude controller
 			_vtol_type->update_mc_state();
-			fill_mc_att_rates_sp();
 
 		} else if (_vtol_type->get_mode() == FIXED_WING) {
 
@@ -692,7 +649,6 @@ void VtolAttitudeControl::task_main()
 			_vtol_vehicle_status.in_transition_to_fw = false;
 
 			_vtol_type->update_fw_state();
-			fill_fw_att_rates_sp();
 
 		} else if (_vtol_type->get_mode() == TRANSITION_TO_MC || _vtol_type->get_mode() == TRANSITION_TO_FW) {
 
@@ -705,7 +661,6 @@ void VtolAttitudeControl::task_main()
 			_vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW);
 
 			_vtol_type->update_transition_state();
-			fill_mc_att_rates_sp();
 		}
 
 		_vtol_type->fill_actuator_outputs();

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -76,7 +76,6 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
 #include "tiltrotor.h"
@@ -128,13 +127,11 @@ private:
 	int	_actuator_inputs_mc{-1};	//topic on which the mc_att_controller publishes actuator inputs
 	int	_airspeed_sub{-1};			// airspeed subscription
 	int	_fw_virtual_att_sp_sub{-1};
-	int	_fw_virtual_v_rates_sp_sub{-1};		//vehicle rates setpoint subscription
 	int	_land_detected_sub{-1};
 	int	_local_pos_sp_sub{-1};			// setpoint subscription
 	int	_local_pos_sub{-1};			// sensor subscription
 	int	_manual_control_sp_sub{-1};	//manual control setpoint subscription
 	int	_mc_virtual_att_sp_sub{-1};
-	int	_mc_virtual_v_rates_sp_sub{-1};		//vehicle rates setpoint subscription
 	int	_params_sub{-1};			//parameter updates subscription
 	int	_pos_sp_triplet_sub{-1};			// local position setpoint subscription
 	int	_tecs_status_sub{-1};
@@ -148,7 +145,6 @@ private:
 	orb_advert_t	_mavlink_log_pub{nullptr};	// mavlink log uORB handle
 	orb_advert_t	_v_att_sp_pub{nullptr};
 	orb_advert_t	_v_cmd_ack_pub{nullptr};
-	orb_advert_t	_v_rates_sp_pub{nullptr};
 	orb_advert_t	_vtol_vehicle_status_pub{nullptr};
 	orb_advert_t 	_actuators_1_pub{nullptr};
 
@@ -199,6 +195,8 @@ private:
 		param_t front_trans_timeout;
 		param_t mpc_xy_cruise;
 		param_t fw_motors_off;
+		param_t diff_thrust;
+		param_t diff_thrust_scale;
 	} _params_handles{};
 
 	/* for multicopters it is usual to have a non-zero idle speed of the engines
@@ -229,10 +227,7 @@ private:
 	void 		vehicle_local_pos_poll();		// Check for changes in sensor values
 	void 		vehicle_local_pos_sp_poll();		// Check for changes in setpoint values
 
-	int 		parameters_update();			//Update local paraemter cache
-
-	void 		fill_mc_att_rates_sp();
-	void 		fill_fw_att_rates_sp();
+	int 		parameters_update();			//Update local parameter cache
 
 	void		handle_command();
 };

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -289,3 +289,39 @@ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0);
+
+/**
+ * Differential thrust in forwards flight.
+ *
+ * Set to 1 to enable differential thrust in fixed-wing flight.
+ *
+ * @min 0
+ * @max 1
+ * @decimal 0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0);
+
+/**
+ * Differential thrust scaling factor
+ *
+ * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f);
+
+/**
+ * Airspeed rules regarding fixed wing control surface scaling.
+ *
+ * @value 0 No special rules
+ * @value 1 During hover (excluding transitions) use the lowest possible airspeed value.
+ * @min 0.0
+ * @increment 1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_AIRSPD_RULE, 0);

src/modules/vtol_att_control/vtol_type.h

@@ -68,6 +68,8 @@ struct Params {
 	float front_trans_timeout;
 	float mpc_xy_cruise;
 	int32_t fw_motors_off;			/**< bitmask of all motors that should be off in fixed wing mode */
+	int32_t diff_thrust;
+	float diff_thrust_scale;
 };
 
 // Has to match 1:1 msg/vtol_vehicle_status.msg