diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 7a18d2435e..0e8040caf2 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -226,24 +226,30 @@ void VtolType::check_quadchute_condition() } // adaptive quadchute - // We use tecs for tracking in FW and local_pos_sp during transitions if (_params->fw_alt_err > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) { - float altErr = 0.0f; + // We use tecs for tracking in FW and local_pos_sp during transitions if (_tecs_running) { - altErr = _tecs_status->altitudeSp - _tecs_status->altitude_filtered; + // 1 second rolling average + _ra_hrate = (49 * _ra_hrate + _tecs_status->flightPathAngle) / 50; + _ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->flightPathAngleSp) / 50; + + // are we dropping while requesting significant ascend? + if (((_tecs_status->altitudeSp - _tecs_status->altitude_filtered) > _params->fw_alt_err) && + (_ra_hrate < -1.0f) && + (_ra_hrate_sp > 1.0f)) { + + _attc->abort_front_transition("QuadChute: loss of altitude"); + } } else { - altErr = -_local_pos_sp->z - -_local_pos->z; + const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _params->fw_alt_err); + 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_valid) { - altErr = 0.0f; + if (height_error && height_rate_error) { + _attc->abort_front_transition("QuadChute: large altitude error"); } } - - if (altErr > _params->fw_alt_err) { - _attc->abort_front_transition("QuadChute: Altitude error too large"); - } } // fixed-wing maximum pitch angle diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index b7f82c9c48..b950203f11 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -184,9 +184,13 @@ protected: float _mc_pitch_weight = 1.0f; // weight for multicopter attitude controller pitch output float _mc_yaw_weight = 1.0f; // weight for multicopter attitude controller yaw output float _mc_throttle_weight = 1.0f; // weight for multicopter throttle command. Used to avoid + // motors spinning up or cutting too fast when doing transitions. float _thrust_transition = 0.0f; // thrust value applied during a front transition (tailsitter & tiltrotor only) + float _ra_hrate = 0.0f; // rolling average on height rate for quadchute condition + float _ra_hrate_sp = 0.0f; // rolling average on height rate setpoint for quadchute condition + bool _flag_was_in_trans_mode = false; // true if mode has just switched to transition hrt_abstime _trans_finished_ts = 0; bool _tecs_running = false;