From d34767b871e14e9ac5cea708ec35bcfdb1e6bbb3 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 5 Sep 2017 12:09:49 +0200 Subject: [PATCH] VTOL standatd - Simplify multicopter weight in update_transition_state() --- src/modules/vtol_att_control/standard.cpp | 56 +++++++---------------- 1 file changed, 16 insertions(+), 40 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 54000ad79f..eafe3f2e21 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -278,6 +278,8 @@ void Standard::update_vtol_state() void Standard::update_transition_state() { + float mc_weight = 1.0f; + VtolType::update_transition_state(); // copy virtual attitude setpoint to real attitude setpoint @@ -299,40 +301,22 @@ void Standard::update_transition_state() _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_blend && (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f) ) { - float weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) / - _airspeed_trans_blend_margin; - _mc_roll_weight = weight; - _mc_pitch_weight = weight; - _mc_yaw_weight = weight; - _mc_throttle_weight = weight; - + mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) / + _airspeed_trans_blend_margin; // time based blending when no airspeed sensor is set } else if (_params_standard.airspeed_disabled && hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1e6f) && hrt_elapsed_time(&_vtol_schedule.transition_start) > ((_params_standard.front_trans_time_min / 2.0f) * 1e6f) ) { - float weight = 1.0f - ((hrt_elapsed_time(&_vtol_schedule.transition_start) - (( - _params_standard.front_trans_time_min / 2.0f) * 1e6f)) / - ((_params_standard.front_trans_time_min / 2.0f) * 1e6f)); + mc_weight = 1.0f - ((float)(hrt_elapsed_time(&_vtol_schedule.transition_start) - (( + _params_standard.front_trans_time_min / 2.0f) * 1000000.0f)) / + ((_params_standard.front_trans_time_min / 2.0f) * 1000000.0f)); - weight = math::constrain(weight, 0.0f, 1.0f); - - _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; } // ramp up FW_PSP_OFF - _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - _mc_pitch_weight); + _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight); matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _v_att_sp->q_d_valid = true; @@ -367,19 +351,9 @@ void Standard::update_transition_state() // continually increase mc attitude control as we transition back to mc mode if (_params_standard.back_trans_ramp > FLT_EPSILON) { - float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / - ((_params_standard.back_trans_ramp) * 1000000.0f); - weight = math::constrain(weight, 0.0f, 1.0f); - _mc_roll_weight = weight; - _mc_pitch_weight = weight; - _mc_yaw_weight = weight; - _mc_throttle_weight = weight; + mc_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + ((_params_standard.back_trans_ramp) * 1000000.0f); - } 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 @@ -390,10 +364,12 @@ 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); + mc_weight = math::constrain(mc_weight, 0.0f, 1.0f); + + _mc_roll_weight = mc_weight; + _mc_pitch_weight = mc_weight; + _mc_yaw_weight = mc_weight; + _mc_throttle_weight = mc_weight; } void Standard::update_mc_state()