diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 781e73093b..7ecfb1f6a5 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -135,26 +135,8 @@ void Standard::update_vtol_state() mc_weight = 0.0f; } else if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) { - // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode - const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) - && !_param_fw_arsp_mode.get(); - const bool minimum_trans_time_elapsed = _time_since_trans_start > getMinimumFrontTransitionTime(); - - bool transition_to_fw = false; - - if (minimum_trans_time_elapsed) { - if (airspeed_triggers_transition) { - transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get(); - - } else { - transition_to_fw = true; - } - } - - transition_to_fw |= can_transition_on_ground(); - - if (transition_to_fw) { + if (isFrontTransitionCompleted()) { _vtol_mode = vtol_mode::FW_MODE; // don't set pusher throttle here as it's being ramped up elsewhere diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 97e8a76978..026c648ebc 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -69,7 +69,6 @@ void Tailsitter::update_vtol_state() * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. */ - float pitch = Eulerf(Quatf(_v_att->q)).theta(); if (_vtol_vehicle_status->fixed_wing_system_failure) { // Failsafe event, switch to MC mode immediately @@ -92,6 +91,7 @@ void Tailsitter::update_vtol_state() break; case vtol_mode::TRANSITION_BACK: + const float pitch = Eulerf(Quatf(_v_att->q)).theta(); // check if we have reached pitch angle to switch to MC mode if (pitch >= PITCH_TRANSITION_BACK || _time_since_trans_start > _param_vt_b_trans_dur.get()) { @@ -115,23 +115,7 @@ void Tailsitter::update_vtol_state() case vtol_mode::TRANSITION_FRONT_P1: { - const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) - && !_param_fw_arsp_mode.get() ; - - bool transition_to_fw = false; - - if (pitch <= PITCH_TRANSITION_FRONT_P1) { - if (airspeed_triggers_transition) { - transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ; - - } else { - transition_to_fw = true; - } - } - - transition_to_fw |= can_transition_on_ground(); - - if (transition_to_fw) { + if (isFrontTransitionCompleted()) { _vtol_mode = vtol_mode::FW_MODE; } @@ -370,3 +354,24 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time(); } + + +bool Tailsitter::isFrontTransitionCompletedBase() +{ + const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) + && !_param_fw_arsp_mode.get() ; + + bool transition_to_fw = false; + const float pitch = Eulerf(Quatf(_v_att->q)).theta(); + + if (pitch <= PITCH_TRANSITION_FRONT_P1) { + if (airspeed_triggers_transition) { + transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ; + + } else { + transition_to_fw = true; + } + } + + return transition_to_fw; +} diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 295f95c8d3..965186a1c0 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -79,6 +79,8 @@ private: void parameters_update() override; + bool isFrontTransitionCompletedBase() override; + DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType, (ParamFloat) _param_fw_psp_off ) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 86a92226c7..f561327496 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -134,25 +134,7 @@ void Tiltrotor::update_vtol_state() break; case vtol_mode::TRANSITION_FRONT_P1: { - - const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) - && !_param_fw_arsp_mode.get() ; - - bool transition_to_p2 = false; - - if (_time_since_trans_start > getMinimumFrontTransitionTime()) { - if (airspeed_triggers_transition) { - transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ; - - } else { - transition_to_p2 = _tilt_control >= _param_vt_tilt_trans.get() && - _time_since_trans_start > getOpenLoopFrontTransitionTime(); - } - } - - transition_to_p2 |= can_transition_on_ground(); - - if (transition_to_p2) { + if (isFrontTransitionCompleted()) { _vtol_mode = vtol_mode::TRANSITION_FRONT_P2; resetTransitionStates(); } @@ -526,3 +508,8 @@ float Tiltrotor::moveLinear(float start, float stop, float progress) { return start + progress * (stop - start); } + +bool Tiltrotor::isFrontTransitionCompletedBase() +{ + return VtolType::isFrontTransitionCompletedBase() && _tilt_control >= _param_vt_tilt_trans.get(); +} diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 191c980e09..e08796c846 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -84,7 +84,7 @@ private: float moveLinear(float start, float stop, float progress); void blendThrottleDuringBacktransition(const float scale, const float target_throttle); - + bool isFrontTransitionCompletedBase() override; hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */ bool _tilt_motors_for_startup{false}; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index a7055c6d89..3ab8c79b8c 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -207,6 +207,35 @@ float VtolType::update_and_get_backtransition_pitch_sp() return math::constrain(pitch_sp_new, 0.f, pitch_lim); } +bool VtolType::isFrontTransitionCompleted() +{ + bool ret = isFrontTransitionCompletedBase(); + + return ret || can_transition_on_ground(); +} + +bool VtolType::isFrontTransitionCompletedBase() +{ + // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode + const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) + && !_param_fw_arsp_mode.get(); + const bool minimum_trans_time_elapsed = _time_since_trans_start > getMinimumFrontTransitionTime(); + const bool openloop_trans_time_elapsed = _time_since_trans_start > getOpenLoopFrontTransitionTime(); + + bool transition_to_fw = false; + + if (airspeed_triggers_transition) { + transition_to_fw = minimum_trans_time_elapsed + && _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get(); + + } else { + transition_to_fw = openloop_trans_time_elapsed; + } + + return transition_to_fw; + +} + bool VtolType::can_transition_on_ground() { return !_v_control_mode->flag_armed || _land_detected->landed; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 3f9ba2ade9..aa6b9128f3 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -303,6 +303,8 @@ protected: bool _quadchute_command_treated{false}; float update_and_get_backtransition_pitch_sp(); + bool isFrontTransitionCompleted(); + virtual bool isFrontTransitionCompletedBase(); SlewRate _spoiler_setpoint_with_slewrate; SlewRate _flaps_setpoint_with_slewrate;