diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 8a5896999e..c5a015eb4d 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -54,6 +54,7 @@ Standard::Standard(VtolAttitudeControl *attc) : _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; _params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR"); _params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR"); @@ -113,6 +114,7 @@ void Standard::update_vtol_state() _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; } else if (_vtol_schedule.flight_mode == FW_MODE) { // transition to mc mode @@ -126,6 +128,7 @@ void Standard::update_vtol_state() _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // transition to MC mode if transition time has passed @@ -151,6 +154,7 @@ void Standard::update_vtol_state() _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; _mc_yaw_weight = 0.0f; + _mc_throttle_weight = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode @@ -207,12 +211,14 @@ void Standard::update_transition_state() _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; + _mc_throttle_weight = weight; } else { // at low speeds give full weight to mc _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { @@ -223,11 +229,13 @@ void Standard::update_transition_state() _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; + _mc_throttle_weight = weight; } else { _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; } // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again @@ -241,6 +249,7 @@ void Standard::update_transition_state() _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); _mc_pitch_weight = math::constrain(_mc_pitch_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); } void Standard::update_mc_state() @@ -280,7 +289,7 @@ void Standard::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; // yaw _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; // throttle /* fixed wing controls */ _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 106f91a8e4..e7daa2c3a3 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -178,7 +178,7 @@ void Tailsitter::update_vtol_state() // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans - && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) { + && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) { _vtol_schedule.flight_mode = FW_MODE; //_vtol_schedule.transition_start = hrt_absolute_time(); } @@ -461,7 +461,8 @@ void Tailsitter::fill_actuator_outputs() // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon } break; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 196751016c..2f9211885b 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -117,6 +117,8 @@ protected: float _mc_roll_weight; // weight for multicopter attitude controller roll output float _mc_pitch_weight; // weight for multicopter attitude controller pitch output float _mc_yaw_weight; // weight for multicopter attitude controller yaw output + float _mc_throttle_weight; // weight for multicopter throttle command. Used to avoid + // motors spinning up or cutting too fast whend doing transitions. float _yaw_transition; // yaw angle in which transition will take place float _pitch_transition_start; // pitch angle at the start of transition (tailsitter)