diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 270aa065a9..c4fa37eb2c 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/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() diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 0248a20727..5725139ef4 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/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) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index c10f7b0dd7..2e68d5d85d 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/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 diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index d8557110c0..4345f506a1 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/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