diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 9be4ce5b3e..f8eee38ba7 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -204,16 +204,16 @@ void Standard::update_transition_state() _param_vt_psher_slew.get() * _dt, _param_vt_f_trans_thr.get()); } - _airspeed_trans_blend_margin = _param_vt_arsp_trans.get() - _param_vt_arsp_blend.get(); + _airspeed_trans_blend_margin = getTransitionAirspeed() - getBlendAirspeed(); // do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed if (_airspeed_trans_blend_margin > 0.0f && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && _airspeed_validated->calibrated_airspeed_m_s > 0.0f && - _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get() && + _airspeed_validated->calibrated_airspeed_m_s >= getBlendAirspeed() && _time_since_trans_start > getMinimumFrontTransitionTime()) { - mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) / + mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - getBlendAirspeed()) / _airspeed_trans_blend_margin; // time based blending when no airspeed sensor is set diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index c06d7e888e..0db029c161 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -244,9 +244,9 @@ void Tiltrotor::update_transition_state() _mc_yaw_weight = 1.0f; if (!_param_fw_arsp_mode.get() && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && - _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get()) { - const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) / - (_param_vt_arsp_trans.get() - _param_vt_arsp_blend.get()); + _airspeed_validated->calibrated_airspeed_m_s >= getBlendAirspeed()) { + const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - getBlendAirspeed()) / + (getTransitionAirspeed() - getBlendAirspeed()); _mc_roll_weight = weight; _mc_yaw_weight = weight; } diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 0a558b0ec6..85e928df0a 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -50,6 +50,11 @@ using namespace matrix; #define THROTTLE_BLENDING_DUR_S 1.0f +// [.] minimum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived) +static constexpr float kMinWeightRatio = 0.5f; + +// [.] maximum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived) +static constexpr float kMaxWeightRatio = 2.0f; VtolType::VtolType(VtolAttitudeControl *att_controller) : ModuleParams(nullptr), @@ -210,7 +215,7 @@ bool VtolType::isFrontTransitionCompletedBase() if (airspeed_triggers_transition) { transition_to_fw = minimum_trans_time_elapsed - && _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get(); + && _airspeed_validated->calibrated_airspeed_m_s >= getTransitionAirspeed(); } else { transition_to_fw = openloop_trans_time_elapsed; @@ -581,3 +586,21 @@ float VtolType::getOpenLoopFrontTransitionTime() const { return getFrontTransitionTimeFactor() * _param_vt_f_tr_ol_tm.get(); } +float VtolType::getTransitionAirspeed() const +{ + return math::max(_param_vt_arsp_trans.get(), getMinimumAirspeed()); +} +float VtolType::getMinimumAirspeed() const +{ + float weight_ratio = 1.0f; + + if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) { + weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), kMinWeightRatio, kMaxWeightRatio); + } + + return sqrtf(weight_ratio) * _param_airspeed_min.get(); +} +float VtolType::getBlendAirspeed() const +{ + return _param_vt_arsp_blend.get(); +} diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 84dfbe6d00..acfbeace88 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -237,6 +237,24 @@ public: */ float getOpenLoopFrontTransitionTime() const; + /** + * + * @return The minimum calibrated airspeed compensated for weight [m/s] + */ + float getMinimumAirspeed() const; + + /** + * + * @return The calibrated blending airspeed [m/s] + */ + float getBlendAirspeed() const; + + /** + * + * @return The calibrated transition airspeed [m/s] + */ + float getTransitionAirspeed() const; + virtual void parameters_update() = 0; /** @@ -343,7 +361,11 @@ protected: (ParamInt) _param_vt_fwd_thrust_en, (ParamFloat) _param_mpc_land_alt1, (ParamFloat) _param_mpc_land_alt2, - (ParamFloat) _param_vt_lnd_pitch_min + (ParamFloat) _param_vt_lnd_pitch_min, + (ParamFloat) _param_weight_base, + (ParamFloat) _param_weight_gross, + (ParamFloat) _param_airspeed_min + ) private: