fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 13:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

vtol_att_control: Consolidate logic for front transiton completion (#21107)

Browse Source 
* vtol_att_control: consolidate logic for front transiton completion

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* addressed review comments

Signed-off-by: RomanBapst <bapstroman@gmail.com>

---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Roman Bapst
2023-02-24 13:52:27 +01:00
committed by GitHub
parent 4259b5adac
commit 33b54f7c57
7 changed files with 64 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/standard.cpp
+1 -19
View File
@@ -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
src/modules/vtol_att_control/tailsitter.cpp
+23 -18
View File
@@ -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;
}
src/modules/vtol_att_control/tailsitter.h
+2
View File
@@ -79,6 +79,8 @@ private:
void parameters_update() override;
bool isFrontTransitionCompletedBase() override;
DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off
)
src/modules/vtol_att_control/tiltrotor.cpp
+6 -19
View File
@@ -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();
}
src/modules/vtol_att_control/tiltrotor.h
+1 -1
View File
@@ -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};
src/modules/vtol_att_control/vtol_type.cpp
+29
View File
@@ -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;
src/modules/vtol_att_control/vtol_type.h
+2
View File
@@ -303,6 +303,8 @@ protected:
bool _quadchute_command_treated{false};
float update_and_get_backtransition_pitch_sp();
bool isFrontTransitionCompleted();
virtual bool isFrontTransitionCompletedBase();
SlewRate<float> _spoiler_setpoint_with_slewrate;
SlewRate<float> _flaps_setpoint_with_slewrate;