diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 97a1e0020d..a0e9692444 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -68,7 +68,6 @@ Standard::Standard(VtolAttitudeControl *attc) : _params_handles_standard.front_trans_time_min = param_find("VT_TRANS_MIN_TM"); _params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); _params_handles_standard.forward_thurst_scale = param_find("VT_FWD_THRUST_SC"); - _params_handles_standard.fw_minimum_altitude = param_find("VT_FW_MIN_ALT"); } Standard::~Standard() @@ -115,9 +114,6 @@ Standard::parameters_update() /* scale for fixed wing thrust used for forward acceleration in multirotor mode */ param_get(_params_handles_standard.forward_thurst_scale, &_params_standard.forward_thurst_scale); - /* QuadChute; minimum altitude for fixed wing flight */ - param_get(_params_handles_standard.fw_minimum_altitude, &_params_standard.fw_minimum_altitude); - return OK; } @@ -262,12 +258,6 @@ void Standard::update_transition_state() } } - // quadchute - if(_params_standard.fw_minimum_altitude > FLT_EPSILON && _armed->armed){ - if(-(_local_pos->z) < _params_standard.fw_minimum_altitude){ - _attc->abort_front_transition(); - } - } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { @@ -373,13 +363,6 @@ void Standard::update_fw_state() set_idle_fw(); // force them to stop, not just idle _flag_enable_mc_motors = true; } - - // quadchute - if(_params_standard.fw_minimum_altitude > FLT_EPSILON && _armed->armed){ - if(-(_local_pos->z) < _params_standard.fw_minimum_altitude){ - _attc->abort_front_transition(); - } - } } /** diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 3a3e22e64c..ecddb92fcd 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -75,8 +75,7 @@ private: float front_trans_timeout; float front_trans_time_min; float down_pitch_max; - float forward_thurst_scale; - float fw_minimum_altitude; + float forward_thurst_scale; } _params_standard; struct { @@ -89,7 +88,6 @@ private: param_t front_trans_time_min; param_t down_pitch_max; param_t forward_thurst_scale; - param_t fw_minimum_altitude; } _params_handles_standard; enum vtol_mode { 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 fb6154e963..5cfd3da4b9 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -134,6 +134,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + _params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT"); /* fetch initial parameter values */ parameters_update(); @@ -567,6 +568,11 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.elevons_mc_lock, &l); _params.elevons_mc_lock = l; + /* minimum relative altitude for FW mode (QuadChute) */ + param_get(_params_handles.fw_min_alt, &v); + _params.fw_min_alt = v; + + return OK; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index e2a63993d7..90a36e652f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -209,6 +209,7 @@ private: param_t arsp_lp_gain; param_t vtol_type; param_t elevons_mc_lock; + param_t fw_min_alt; } _params_handles; /* for multicopters it is usual to have a non-zero idle speed of the engines diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index c9e69d7b62..73d90fb3b3 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -177,6 +177,25 @@ void VtolType::update_fw_state() if (!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts)) { waiting_on_tecs(); } + + // quadchute + if(_params->fw_min_alt > FLT_EPSILON && _armed->armed){ + if(-(_local_pos->z) < _params->fw_min_alt){ + _attc->abort_front_transition(); + } + } + +} + +void VtolType::update_transition_state() +{ + // quadchute + if(_params->fw_min_alt > FLT_EPSILON && _armed->armed){ + if(-(_local_pos->z) < _params->fw_min_alt){ + _attc->abort_front_transition(); + } + } + } bool VtolType::can_transition_on_ground() diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 9a15ff8cfa..d755fb43e4 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -59,6 +59,7 @@ struct Params { float arsp_lp_gain; // total airspeed estimate low pass gain int vtol_type; int elevons_mc_lock; // lock elevons in multicopter mode + float fw_min_alt; // minimum relative altitude for FW mode (QuadChute) }; enum mode {