From ec15130a80b702762a58e3309c49bce4b0b62f32 Mon Sep 17 00:00:00 2001 From: sander Date: Fri, 13 May 2016 01:46:24 +0200 Subject: [PATCH] Minimum altitude for FW flight in standard VTOL --- src/modules/vtol_att_control/standard.cpp | 8 ++++++++ src/modules/vtol_att_control/standard.h | 2 ++ src/modules/vtol_att_control/standard_params.c | 12 ++++++++++++ 3 files changed, 22 insertions(+) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 9109a82f90..1815676321 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -68,6 +68,7 @@ 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() @@ -114,6 +115,9 @@ 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; } @@ -361,6 +365,10 @@ void Standard::update_fw_state() set_idle_fw(); // force them to stop, not just idle _flag_enable_mc_motors = true; } + + if(_local_pos->dist_bottom < _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 b4ab84d59b..3a3e22e64c 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -76,6 +76,7 @@ private: float front_trans_time_min; float down_pitch_max; float forward_thurst_scale; + float fw_minimum_altitude; } _params_standard; struct { @@ -88,6 +89,7 @@ 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/standard_params.c b/src/modules/vtol_att_control/standard_params.c index b4ab3e4c42..114dc17474 100644 --- a/src/modules/vtol_att_control/standard_params.c +++ b/src/modules/vtol_att_control/standard_params.c @@ -72,3 +72,15 @@ PARAM_DEFINE_FLOAT(VT_DWN_PITCH_MAX, 5.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_FWD_THRUST_SC, 0.0f); + + +/** + * QuadChute + * + * Minimum altitude for fixed wing flight, when in fixed wing the altitude drops below this altitude + * the vehicle will transition back to MC mode and enter failsafe RTL + * @min 0.0 + * @max 200.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);