VTOL: standard VTOL and Tiltrotor manual MC throttle during transition

This sets the throttle of the Multicopter actuators to the throttle stick position
while in Stabilized, Acro or Manual mode.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/vtol_att_control/standard.cpp

@@ -267,6 +267,11 @@ void Standard::update_transition_state()
 
 		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
 
+		// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
+		if (!_v_control_mode->flag_control_climb_rate_enabled) {
+			_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
+		}
+
 		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);
 
@@ -287,6 +292,11 @@ void Standard::update_transition_state()
 			_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
 		}
 
+		// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
+		if (!_v_control_mode->flag_control_climb_rate_enabled) {
+			_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
+		}
+
 		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);
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -322,6 +322,11 @@ void Tiltrotor::update_transition_state()
 
 		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
 
+		// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
+		if (!_v_control_mode->flag_control_climb_rate_enabled) {
+			_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
+		}
+
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) {
 		// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
 		_tilt_control = _params_tiltrotor.tilt_transition +
@@ -341,6 +346,11 @@ void Tiltrotor::update_transition_state()
 
 		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
 
+		// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
+		if (!_v_control_mode->flag_control_climb_rate_enabled) {
+			_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
+		}
+
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
 		// turn on all MC motors
 		set_all_motor_state(motor_state::ENABLED);
@@ -376,6 +386,11 @@ void Tiltrotor::update_transition_state()
 			// slowly ramp up throttle to avoid step inputs
 			_mc_throttle_weight = (time_since_trans_start - 1.0f) / 1.0f;
 		}
+
+		// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
+		if (!_v_control_mode->flag_control_climb_rate_enabled) {
+			_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
+		}
 	}
 
 	const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));