diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index effb4c5f56..781e73093b 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -89,7 +89,7 @@ void Standard::update_vtol_state() } else if (_vtol_mode == vtol_mode::FW_MODE) { // Regular backtransition - resetTransitionTimer(); + resetTransitionStates(); _vtol_mode = vtol_mode::TRANSITION_TO_MC; _reverse_output = _param_vt_b_rev_out.get(); @@ -126,7 +126,7 @@ void Standard::update_vtol_state() // start transition to fw mode /* NOTE: The failsafe transition to fixed-wing was removed because it can result in an * unsafe flying state. */ - resetTransitionTimer(); + resetTransitionStates(); _vtol_mode = vtol_mode::TRANSITION_TO_FW; } else if (_vtol_mode == vtol_mode::FW_MODE) { diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index b82e24dd61..97e8a76978 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -82,7 +82,7 @@ void Tailsitter::update_vtol_state() break; case vtol_mode::FW_MODE: - resetTransitionTimer(); + resetTransitionStates(); _vtol_mode = vtol_mode::TRANSITION_BACK; break; @@ -107,7 +107,7 @@ void Tailsitter::update_vtol_state() case vtol_mode::MC_MODE: // initialise a front transition _vtol_mode = vtol_mode::TRANSITION_FRONT_P1; - resetTransitionTimer(); + resetTransitionStates(); break; case vtol_mode::FW_MODE: diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index e62fed0cb1..86a92226c7 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -83,7 +83,7 @@ void Tiltrotor::update_vtol_state() break; case vtol_mode::FW_MODE: - resetTransitionTimer(); + resetTransitionStates(); _vtol_mode = vtol_mode::TRANSITION_BACK; break; @@ -126,7 +126,7 @@ void Tiltrotor::update_vtol_state() switch (_vtol_mode) { case vtol_mode::MC_MODE: // initialise a front transition - resetTransitionTimer(); + resetTransitionStates(); _vtol_mode = vtol_mode::TRANSITION_FRONT_P1; break; @@ -154,7 +154,7 @@ void Tiltrotor::update_vtol_state() if (transition_to_p2) { _vtol_mode = vtol_mode::TRANSITION_FRONT_P2; - resetTransitionTimer(); + resetTransitionStates(); } break; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e29e1a32fd..3830932ff1 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -219,10 +219,10 @@ VtolAttitudeControl::quadchute(QuadchuteReason reason) "Quad-chute triggered due to loss of altitude"); break; - case QuadchuteReason::LargeAltError: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: large altitude error\t"); - events::send(events::ID("vtol_att_ctrl_quadchute_alt_err"), events::Log::Critical, - "Quad-chute triggered due to large altitude error"); + case QuadchuteReason::TransitionAltitudeLoss: + mavlink_log_critical(&_mavlink_log_pub, "Quadchute: loss of altitude during transition\t"); + events::send(events::ID("vtol_att_ctrl_quadchute_trans_alt_err"), events::Log::Critical, + "Quad-chute triggered due to loss of altitude during transition"); break; case QuadchuteReason::MaximumPitchExceeded: diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 117e557a94..85be3cbf97 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -226,6 +226,24 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f); */ PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f); +/** + * Quad-chute transition altitude loss threshold + * + * Altitude loss threshold for quad-chute triggering during VTOL transition to fixed-wing flight. + * If the current altitude is more than this value below the altitude at the beginning of the + * transition, it will instantly switch back to MC mode and execute behavior defined in COM_QC_ACT. + * + * Set to 0 do disable this threshold. + * + * @unit m + * @min 0 + * @max 50 + * @increment 1 + * @decimal 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_QC_T_ALT_LOSS, 10.0f); + /** * Quad-chute max pitch threshold * diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 6fb7e3482c..0226a880f8 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -212,6 +212,13 @@ bool VtolType::can_transition_on_ground() return !_v_control_mode->flag_armed || _land_detected->landed; } +void VtolType::resetTransitionStates() +{ + _transition_start_timestamp = hrt_absolute_time(); + _time_since_trans_start = 0.f; + _local_position_z_start_of_transition = _local_pos->z; +} + bool VtolType::isQuadchuteEnabled() { float dist_to_ground = 0.f; @@ -266,21 +273,23 @@ bool VtolType::largeAltitudeLoss() return false; } -bool VtolType::largeAltitudeError() +bool VtolType::isFrontTransitionAltitudeLoss() { - // adaptive quadchute - if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled && !_tecs_running) { + bool result = false; + if (_param_vt_qc_t_alt_loss.get() > FLT_EPSILON && _common_vtol_mode == mode::TRANSITION_TO_FW && _local_pos->z_valid) { - const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _param_vt_fw_alt_err.get()); - const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f); + if (_local_pos->z <= FLT_EPSILON) { + // vehilce is above home + result = _local_pos->z - _local_position_z_start_of_transition > _param_vt_qc_t_alt_loss.get(); - if (height_error && height_rate_error) { - return true; + } else { + // vehilce is below home + result = _local_position_z_start_of_transition - _local_pos->z > _param_vt_qc_t_alt_loss.get(); } } - return false; + return result; } bool VtolType::isPitchExceeded() @@ -335,8 +344,8 @@ QuadchuteReason VtolType::getQuadchuteReason() return QuadchuteReason::LossOfAlt; } - if (largeAltitudeError()) { - return QuadchuteReason::LargeAltError; + if (isFrontTransitionAltitudeLoss()) { + return QuadchuteReason::TransitionAltitudeLoss; } if (isPitchExceeded()) { diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 33dc2b19b8..e062ec340d 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -90,7 +90,7 @@ enum class QuadchuteReason { ExternalCommand, MinimumAltBreached, LossOfAlt, - LargeAltError, + TransitionAltitudeLoss, MaximumPitchExceeded, MaximumRollExceeded, }; @@ -173,11 +173,11 @@ public: bool largeAltitudeLoss(); /** - * @brief Indicates if the vehicle has an altitude error larger than VT_FW_ALT_ERR. This only applied when TECS is not running. + * @brief Indicates if there is an altitude loss higher than specified threshold during a VTOL transition to FW * - * @return true if error larger than threshold + * @return true if error larger than threshold */ - bool largeAltitudeError(); + bool isFrontTransitionAltitudeLoss(); /** * @brief Indicates if the absolute value of the vehicle pitch angle exceeds the threshold defined by VT_FW_QC_P @@ -252,11 +252,7 @@ public: * @brief Resets the transition timer states. * */ - void resetTransitionTimer() - { - _transition_start_timestamp = hrt_absolute_time(); - _time_since_trans_start = 0.f; - } + void resetTransitionStates(); protected: VtolAttitudeControl *_attc; @@ -318,12 +314,15 @@ protected: float _dt{0.0025f}; // time step [s] + float _local_position_z_start_of_transition{0.f}; // altitude at start of transition + DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, (ParamBool) _param_vt_elev_mc_lock, (ParamFloat) _param_vt_fw_min_alt, (ParamFloat) _param_vt_fw_alt_err, (ParamInt) _param_vt_fw_qc_p, (ParamInt) _param_vt_fw_qc_r, + (ParamFloat) _param_vt_qc_t_alt_loss, (ParamInt) _param_quadchute_max_height, (ParamFloat) _param_vt_f_tr_ol_tm, (ParamFloat) _param_vt_trans_min_tm,