tiltrotor: publish attitude setpoint when doing a transition

Conflicts:
	src/modules/vtol_att_control/vtol_att_control_main.h

src/modules/vtol_att_control/tiltrotor.cpp

@@ -56,6 +56,8 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
 	_mc_pitch_weight = 1.0f;
 	_mc_yaw_weight = 1.0f;
 
+	_flag_was_in_trans_mode = false;
+
 	_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");
@@ -292,6 +294,12 @@ void Tiltrotor::update_fw_state()
 
 void Tiltrotor::update_transition_state()
 {
+	if (!_flag_was_in_trans_mode) {
+		// save desired heading for transition and last thrust value
+		_yaw_transition = _v_att->yaw;
+		_throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
+		_flag_was_in_trans_mode = true;
+	}
 	if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
 		// for the first part of the transition the rear rotors are enabled
 		if (_rear_motors != ENABLED) {
@@ -354,6 +362,23 @@ void Tiltrotor::update_transition_state()
 
 	_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
 	_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
+
+	// compute desired attitude and thrust setpoint for the transition
+	_v_att_sp->timestamp = hrt_absolute_time();
+	_v_att_sp->roll_body = 0;
+	_v_att_sp->pitch_body = 0;
+	_v_att_sp->yaw_body = _yaw_transition;
+	_v_att_sp->thrust = _throttle_transition;
+
+	math::Matrix<3,3> R_sp;
+	R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body);
+	memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body));
+	_v_att_sp->R_valid = true;
+
+	math::Quaternion q_sp;
+	q_sp.from_dcm(R_sp);
+	_v_att_sp->q_d_valid = true;
+	memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
 }
 
 void Tiltrotor::update_external_state()

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -59,7 +59,6 @@ VtolAttitudeControl::VtolAttitudeControl() :
 
 	//init subscription handlers
 	_v_att_sub(-1),
-	_v_att_sp_sub(-1),
 	_mc_virtual_v_rates_sp_sub(-1),
 	_fw_virtual_v_rates_sp_sub(-1),
 	_v_control_mode_sub(-1),
@@ -75,13 +74,13 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_actuators_0_pub(nullptr),
 	_actuators_1_pub(nullptr),
 	_vtol_vehicle_status_pub(nullptr),
-	_v_rates_sp_pub(nullptr)
+	_v_rates_sp_pub(nullptr),
+	_v_att_sp_pub(nullptr)
 
 {
 	memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
 	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/
 	memset(&_v_att, 0, sizeof(_v_att));
-	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
 	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
 	memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp));
 	memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp));
@@ -439,6 +438,17 @@ void VtolAttitudeControl::fill_fw_att_rates_sp()
 	_v_rates_sp.thrust 	= _fw_virtual_v_rates_sp.thrust;
 }
 
+void VtolAttitudeControl::publish_transition_att_sp()
+{
+	if (_v_att_sp_pub != nullptr) {
+		/* publish the attitude setpoint */
+		orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp);
+	} else {
+		/* advertise and publish */
+		_v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
+	}
+}
+
 void
 VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
@@ -451,7 +461,6 @@ void VtolAttitudeControl::task_main()
 	fflush(stdout);
 
 	/* do subscriptions */
-	_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_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));
@@ -599,6 +608,7 @@ void VtolAttitudeControl::task_main()
 			if (got_new_data) {
 				_vtol_type->update_transition_state();
 				fill_mc_att_rates_sp();
+				publish_transition_att_sp();
 			}
 
 		} else if (_vtol_type->get_mode() == EXTERNAL) {

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -151,6 +151,7 @@ private:
 	orb_advert_t 	_actuators_1_pub;
 	orb_advert_t	_vtol_vehicle_status_pub;
 	orb_advert_t	_v_rates_sp_pub;
+	orb_advert_t	_v_att_sp_pub;
 //*******************data containers***********************************************************
 	struct vehicle_attitude_s			_v_att;				//vehicle attitude
 	struct vehicle_attitude_setpoint_s	_v_att_sp;			//vehicle attitude setpoint
@@ -215,6 +216,7 @@ private:
 	void 		fill_mc_att_rates_sp();
 	void 		fill_fw_att_rates_sp();
 	void		handle_command();
+	void 		publish_transition_att_sp();
 };
 
 #endif

src/modules/vtol_att_control/vtol_type.h

@@ -116,6 +116,10 @@ protected:
 	float _mc_pitch_weight;	// weight for multicopter attitude controller pitch output
 	float _mc_yaw_weight;	// weight for multicopter attitude controller yaw output
 
+	float _yaw_transition;	// yaw angle in which transition will take place
+	float _throttle_transition; // throttle value used for the transition phase
+	bool _flag_was_in_trans_mode;	// true if mode has just switched to transition
+
 };
 
 #endif