tiltrotor: enable l1 and deceleration control during transition

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/vtol_att_control/tiltrotor.cpp

@@ -42,6 +42,8 @@
 #include "tiltrotor.h"
 #include "vtol_att_control_main.h"
 
+using namespace matrix;
+
 #define ARSP_YAW_CTRL_DISABLE 7.0f	// airspeed at which we stop controlling yaw during a front transition
 
 Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
@@ -224,6 +226,11 @@ void Tiltrotor::update_transition_state()
 {
 	VtolType::update_transition_state();
 
+	// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
+	_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
+
 	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 	if (!_flag_was_in_trans_mode) {
@@ -310,6 +317,9 @@ void Tiltrotor::update_transition_state()
 
 		_mc_yaw_weight = 1.0f;
 
+		// control backtransition deceleration using pitch.
+		_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
+
 		// while we quickly rotate back the motors keep throttle at idle
 		if (time_since_trans_start < 1.0f) {
 			_mc_throttle_weight = 0.0f;
@@ -324,12 +334,15 @@ void Tiltrotor::update_transition_state()
 		}
 	}
 
+	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);
+
+
 	_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_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f);
 
-	// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
-	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
 }
 
 void Tiltrotor::waiting_on_tecs()