VTOL: Make pusher assist generic to enable also for tiltrotor

-move pusher assist functionality into vtol_type class and adapt it to also
work for tiltrotor VTOLs (pitch rotors down to accelerate forward)
-for tiltrotor: compensate for lost lift due to tilt by increasing the throttle
-enable pusher / tiltassist also in altitude mode

Signed-off-by: Silvan Fuhrer <[email protected]>

Author: sfuhrer

src/modules/vtol_att_control/standard.cpp

@@ -62,8 +62,6 @@ Standard::Standard(VtolAttitudeControl *attc) :
 
 	_params_handles_standard.pusher_ramp_dt = param_find("VT_PSHER_RMP_DT");
 	_params_handles_standard.back_trans_ramp = param_find("VT_B_TRANS_RAMP");
-	_params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
-	_params_handles_standard.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
 	_params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF");
 	_params_handles_standard.reverse_output = param_find("VT_B_REV_OUT");
 	_params_handles_standard.reverse_delay = param_find("VT_B_REV_DEL");
@@ -84,13 +82,6 @@ Standard::parameters_update()
 
 	_airspeed_trans_blend_margin = _params->transition_airspeed - _params->airspeed_blend;
 
-	/* maximum down pitch allowed */
-	param_get(_params_handles_standard.down_pitch_max, &v);
-	_params_standard.down_pitch_max = math::radians(v);
-
-	/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
-	param_get(_params_handles_standard.forward_thrust_scale, &_params_standard.forward_thrust_scale);
-
 	/* pitch setpoint offset */
 	param_get(_params_handles_standard.pitch_setpoint_offset, &v);
 	_params_standard.pitch_setpoint_offset = math::radians(v);
@@ -326,76 +317,9 @@ void Standard::update_mc_state()
 {
 	VtolType::update_mc_state();
 
-	// if the thrust scale param is zero or the drone is on manual mode,
-	// then the pusher-for-pitch strategy is disabled and we can return
-	if (_params_standard.forward_thrust_scale < FLT_EPSILON ||
-	    !_v_control_mode->flag_control_position_enabled) {
-		return;
-	}
-
-	// Do not engage pusher assist during a failsafe event
-	// There could be a problem with the fixed wing drive
-	if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) {
-		return;
-	}
-
-	// disable pusher assist during landing
-	if (_attc->get_pos_sp_triplet()->current.valid
-	    && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
-		return;
-	}
-
-	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
-	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.
-	Dcmf R_yaw = Eulerf(0.0f, 0.0f, -euler(2));
-	body_z_sp = R_yaw * body_z_sp;
-	body_z_sp.normalize();
-
-	// calculate the desired pitch seen in the heading frame
-	// this value corresponds to the amount the vehicle would try to pitch forward
-	float pitch_forward = atan2f(body_z_sp(0), body_z_sp(2));
-
-	// only allow pitching forward up to threshold, the rest of the desired
-	// forward acceleration will be compensated by the pusher
-	if (pitch_forward < -_params_standard.down_pitch_max) {
-		// desired roll angle in heading frame stays the same
-		float roll_new = -asinf(body_z_sp(1));
-
-		_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max))
-				   * _params_standard.forward_thrust_scale;
-
-		// return the vehicle to level position
-		float pitch_new = 0.0f;
-
-		// create corrected desired body z axis in heading frame
-		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));
-		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));
-
-		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;
-	}
+	VtolType::pusher_assist();
 
-	_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
+	_pusher_throttle = _forward_thrust;
 }
 
 void Standard::update_fw_state()

src/modules/vtol_att_control/standard.h

@@ -69,8 +69,6 @@ class Standard : public VtolType
 	struct {
 		float pusher_ramp_dt;
 		float back_trans_ramp;
-		float down_pitch_max;
-		float forward_thrust_scale;
 		float pitch_setpoint_offset;
 		float reverse_output;
 		float reverse_delay;
@@ -79,8 +77,6 @@ class Standard : public VtolType
 	struct {
 		param_t pusher_ramp_dt;
 		param_t back_trans_ramp;
-		param_t down_pitch_max;
-		param_t forward_thrust_scale;
 		param_t pitch_setpoint_offset;
 		param_t reverse_output;
 		param_t reverse_delay;

src/modules/vtol_att_control/tiltrotor.cpp

@@ -207,8 +207,11 @@ void Tiltrotor::update_mc_state()
 {
 	VtolType::update_mc_state();
 
-	// make sure motors are not tilted
-	_tilt_control = _params_tiltrotor.tilt_mc;
+	VtolType::pusher_assist();
+
+	_tilt_control = _forward_thrust;
+
+	Tiltrotor::thrust_compensation_for_tilt();
 }
 
 void Tiltrotor::update_fw_state()
@@ -387,3 +390,20 @@ void Tiltrotor::fill_actuator_outputs()
 			_actuators_fw_in->control[actuator_controls_s::INDEX_YAW];
 	}
 }
+
+/*
+ * Increase combined thrust of MC propellers if motors are tilted. Assumes that all MC motors are tilted equally.
+ */
+
+void Tiltrotor::thrust_compensation_for_tilt()
+{
+
+	// only compensate for tilt angle up to 0.5 * max tilt
+	float compensated_tilt = _tilt_control;
+	compensated_tilt = compensated_tilt < 0.0f ? 0.0f : compensated_tilt;
+	compensated_tilt = compensated_tilt > 0.5f ? 0.5f : compensated_tilt;
+
+	float thrust_new = _v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F);
+
+	_v_att_sp->thrust_body[2] = thrust_new;
+}

src/modules/vtol_att_control/tiltrotor.h

@@ -58,6 +58,7 @@ class Tiltrotor : public VtolType
 	void update_mc_state() override;
 	void update_fw_state() override;
 	void waiting_on_tecs() override;
+	void thrust_compensation_for_tilt();
 
 private:
 

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -86,6 +86,9 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN");
 	_params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
 
+	_params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
+	_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
+
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -272,6 +275,13 @@ VtolAttitudeControl::parameters_update()
 	param_get(_params_handles.diff_thrust_scale, &v);
 	_params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
 
+	/* maximum down pitch allowed */
+	param_get(_params_handles.down_pitch_max, &v);
+	_params.down_pitch_max = math::radians(v);
+
+	/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
+	param_get(_params_handles.forward_thrust_scale, &_params.forward_thrust_scale);
+
 	// make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed
 	_params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f);
 

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -201,6 +201,8 @@ class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public px4::
 		param_t fw_motors_off;
 		param_t diff_thrust;
 		param_t diff_thrust_scale;
+		param_t down_pitch_max;
+		param_t forward_thrust_scale;
 	} _params_handles{};
 
 	/* for multicopters it is usual to have a non-zero idle speed of the engines

src/modules/vtol_att_control/vtol_type.cpp

@@ -46,6 +46,8 @@
 #include <px4_platform_common/defines.h>
 #include <matrix/math.hpp>
 
+using namespace matrix;
+
 
 VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_attc(att_controller),
@@ -187,7 +189,7 @@ void VtolType::check_quadchute_condition()
 {
 
 	if (_v_control_mode->flag_armed && !_land_detected->landed) {
-		matrix::Eulerf euler = matrix::Quatf(_v_att->q);
+		Eulerf euler = Quatf(_v_att->q);
 
 		// fixed-wing minimum altitude
 		if (_params->fw_min_alt > FLT_EPSILON) {
@@ -381,3 +383,81 @@ bool VtolType::is_channel_set(const int channel, const int target)
 
 	return (channel_bitmap >> channel) & 1;
 }
+
+
+void VtolType::pusher_assist()
+{
+
+	// normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0
+	_forward_thrust = 0.0f;
+
+	// if the thrust scale param is zero or the drone is not in some position or altitude control mode,
+	// then the pusher-for-pitch strategy is disabled and we can return
+	if (_params->forward_thrust_scale < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled
+			|| _v_control_mode->flag_control_altitude_enabled)) {
+		return;
+	}
+
+	// Do not engage pusher assist during a failsafe event (could be a problem with the fixed wing drive)
+	if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) {
+		return;
+	}
+
+	// disable pusher assist during landing
+	if (_attc->get_pos_sp_triplet()->current.valid
+	    && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+		return;
+	}
+
+	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);
+
+	// direction of desired body z axis represented in earth frame
+	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.
+	Dcmf R_yaw = Eulerf(0.0f, 0.0f, -euler(2));
+	body_z_sp = R_yaw * body_z_sp;
+	body_z_sp.normalize();
+
+	// calculate the desired pitch seen in the heading frame
+	// this value corresponds to the amount the vehicle would try to pitch down
+	float pitch_down = atan2f(body_z_sp(0), body_z_sp(2));
+
+	// only allow pitching down up to threshold, the rest of the desired
+	// forward acceleration will be compensated by the pusher/tilt
+	if (pitch_down < -_params->down_pitch_max) {
+		// desired roll angle in heading frame stays the same
+		float roll_new = -asinf(body_z_sp(1));
+
+		_forward_thrust = (sinf(-pitch_down) - sinf(_params->down_pitch_max)) * _params->forward_thrust_scale;
+		// limit forward actuation to [0, 0.9]
+		_forward_thrust = _forward_thrust < 0.0f ? 0.0f : _forward_thrust;
+		_forward_thrust = _forward_thrust > 0.9f ? 0.9f : _forward_thrust;
+
+		// return the vehicle to level position
+		float pitch_new = 0.0f;
+
+		// create corrected desired body z axis in heading frame
+		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));
+		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));
+
+		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;
+	}
+
+}

src/modules/vtol_att_control/vtol_type.h

@@ -70,6 +70,8 @@ struct Params {
 	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;
+	float down_pitch_max;
+	float forward_thrust_scale;
 };
 
 // Has to match 1:1 msg/vtol_vehicle_status.msg
@@ -163,6 +165,10 @@ class VtolType
 	 */
 	bool can_transition_on_ground();
 
+	/**
+	 * Pusher assist in hover (pusher/pull for standard VTOL, motor tilt for tiltrotor)
+	 */
+	void pusher_assist();
 
 
 	mode get_mode() {return _vtol_mode;}
@@ -216,6 +222,8 @@ class VtolType
 
 	motor_state _motor_state = motor_state::DISABLED;
 
+	float _forward_thrust = 0.0f; // normalized pusher throttle (standard VTOL) or tilt (tiltrotor)
+
 
 
 	/**