dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

vtol: make sure transition airpseed is above weight compensated minimum airspeed 

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2023-09-26 10:12:21 +03:00 committed by Roman Bapst
parent 6dfede0806
commit b50a23beb0
4 changed files with 53 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/vtol_att_control/standard.cpp
View File

@ -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

6
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -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;
}

25
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -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();
}

24
src/modules/vtol_att_control/vtol_type.h
View File

@ -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<px4::params::VT_FWD_THRUST_EN>) _param_vt_fwd_thrust_en,
(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
(ParamFloat<px4::params::VT_LND_PITCH_MIN>) _param_vt_lnd_pitch_min
(ParamFloat<px4::params::VT_LND_PITCH_MIN>) _param_vt_lnd_pitch_min,
(ParamFloat<px4::params::WEIGHT_BASE>) _param_weight_base,
(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_airspeed_min
)
private: